/***********************************************************************
���ݴ�ѧ�������ѧԺ
��ʮ�Ľ���������ܳ�����
*date               2019/6/28
@brief              ͼ��Ѱ�ߴ���
@author             ��β�       <-------- ��һ����
*Software           IAR8.1
This code is my painstaking work and cannot be dissminated without my permission
                                           ----From Kingsoft PowerWord translation
��ʮ�Ľ���������ܳ�����
���ݴ�ѧ�������ѧԺ
************************************************************************/

/***********************************************************************
���ݴ�ѧ�������ѧԺ
��ʮ�����������ܳ�����
*date               2020/-/-
@brief              ͼ��Ѱ�ߴ���
@Second_author      ֣����
*Software           IAR8.1

��ʮ�����������ܳ�����
���ݴ�ѧ�������ѧԺ
************************************************************************/

/***********************************************************************
�������ѧԺ
��ʮ������������ܳ�����
*date               2021/4/16
@brief              ͼ��Ѱ�ߴ���
@Third_author       �����    <-------- ��һ����
*Software           MDK5.28

��ʮ������������ܳ�����
�������ѧԺ
************************************************************************/

#include "zf_common_headfile.h"
extern EulerAngleTypedef       SystemAttitude;            /////��̬��
/******************ͼ�����������*******************/
int hang[6]={35,45,57,65,75,79};//{35,45,57,65,75,79};//��ͼ��//�ɼ���   15  20 25  35  45
int polt_x=0;
//*****************************************************************************//
int left[6];//������ȡǰհ���������
int right[6];//������ȡǰհ���ұ�����
int all_out_left[24];
int all_out_right[24];
int left_bz[24],right_bz[24];
int scan_mid[6];
int edge_Width[6];
int all_out_scan_mid[24];
int all_out_edge_Width[24];
unsigned char flag[6];                 //*************�ұ߱�־  0Ϊ�Ҳ����ߣ�1Ϊ�ҵ������ߣ�2Ϊ�ҵ���ߣ�3Ϊ�ҵ��ұ�
unsigned char all_out_flag[24];        //*************�ұ߱�־  0Ϊ�Ҳ����ߣ�1Ϊ�ҵ������ߣ�2Ϊ�ҵ���ߣ�3Ϊ�ҵ��ұ�
float value[6];
float all_out_value[24];
/**********���մ����ٶȿ��ƺͷ�����Ƶ�ƫ����*********/
float angle_value=0,angle_value_Error=0,angle_value_net=0,angle_value_zu[4];               //����ȷ�����ƫ����
static float angle_value_Old=0,angle_value_Oldd=0;

//**************************Ԫ�ر�־********************//
/***********************ֱ��***************************/
char Line_flag=0;
char Line_add_flag=0;
//float Speed_Line=0;
//float Speed_down=0;
//int line_distance_cnt=0;
/////////////
char AddSpeedCrazy_flag=0;
/***********************ֱ��***************************/
/***********************ʮ��***************************/
char Cross_Flag=0;                  //ʮ�ֱ�־
char flag_left=0,flag_right=0;  //ʮ�����������־
int  Cross_value=0;                         //1Ϊ����2Ϊ����0Ϊ����ʮ�ִ���
int  Cross_old=0;
/***********************ʮ��***************************/
/***********************Բ��***************************/
//Բ����ɨ������
int Ring_left[60] = {0};
int Ring_right[60] = {0};

char  Ring_Flag=0,Ring_in_Flag=0;//������־
int   ring_value=0,Ringint=1;    //������������
float RingAngle=0;
float Ring_plush=0,Ring_En_plush=0,zhi_plush=0;
char  first_in=0;
char  Ring_plush_flag=0;
int   Ring_finish=0;              //��Բ��·��
char  lost_Flag=0;              //��հ��ʧ
float Ring_Roll = 0;                        //�������ٶȻ���
int   ring_value_in =0;         //Բ����ƫ�����޷�
char  ringvlauemax  =0;
/***********************Բ��***************************/
/***********************�µ�***************************/
char  Ramp_Flag=0;                 //�µ���־
float   Ramp_speed = 0;            //�µ������ٶ�
//float   Big_Ramp_Time = 0;         //������ʱ��
int   Ramp_Mode     = 0;         //����ģʽ
int   Ramp_add_time=0;           //�µ���ɺ����¿�������Ԫ�ض���ʱ��
float AGLE=0;
/***********************�µ�***************************/
/******************�ϰ�*************************/
char zhi_flag                =0;
char zhi_value_flag                =0;
int zhi_left1=0,zhi_right1=0;
uint8 zhi_time=0;  //��ʱ
/******************�ϰ�*************************/
/*****************��·*****************/
char duanflag=0;
char bmx_flag=0;//������
uint8 bmx_count=0;
char bmx_zy_flag=0, bmx_value_flag=0;

#define bmx_count_value 200   //����Ȧ��
/*****************��·*****************/
/***********************����***************************/
char  ChangeSpeed = 0;           //�ȼ��ٱ�־
char  Garageout_flag = 0;        //�������־
char  GarageEncoder_Flag = 0;    //����·�̼���ʹ�ܱ�־
int   GarageEncoder = 0;         //����·��
float Garage_Roll=0;             //����ƫ�ƽǶ�
/****///�����ڲ���־λ
char Garageout_step=0;
/****///�����ڲ���־λ
/***********************����***************************/
/***********************�յ�***************************/
char Reversal=0;                //��ɫ��ת��־
char Reversal_num=0;            //��ɫ��ת����
char Destination_Flag=0;        //�յ��־
int  Destination_Encoder=0;     //��������ʻ·��
/***********************�յ�***************************/
/***********************��·***************************/
float ForkEncoder =0;           //��·��ʻ·��
char  ForkRoad_Flag=0;          //��·��־
/***********************��·***************************/
/*********************����ᳵ*************************/
char Rx_Start_Stop_flag=0;       //����׼������
char Rx_Immediately_Stop_flag=0; //��������ͣ��
char Rx_Immediately_Open_flag=0; //������������
int  AimDis_Encoder          =0; //����׼����������ʻ�ľ���
char SanLun_stop_Flag        =0; //����׼���ڶ�������ͣ��
char change_stop_mode        =0; //����ȴ�ģʽ 0ԭ�صȴ����� 1������
int  Sancha_Encoder          =0; //ǿ�ƿ�������·��
char Sancha_count_En         =0; //�����������·�̿��أ�����ǿ�ƿ�������
char Sancha_XianFu           =0; //�������޷�
char Speed_100_Open          =0; //����100�ٶ�

/*********************����ᳵ*************************/
uint16 ForkRoad_count=1;          //����Ԫ���ֶ������־
uint16 Ramp_count    =1;          //�µ�Ԫ���ֶ������־
char   start_flag    =0;          //���ͣ��ʹ�ܱ�־
char DistanceCount_En=0;
//********************ʹ�ܱ�־*******************
char Cross_En     =1;           //ʮ��ʹ��
char Ring_En      =0;           //����ʹ��
char Ramp_En      =1;           //�µ�ʹ��
char GarageOut_En =0;                       //����ʹ��
char GarageIn_En  =0;                       //����ʹ��
char ForkRoad_En  =0;                       //��·ʹ��

char Turn_Ball_En =1;                       //����ʹ��
char Crazy_Add    =1;           //쭳�ʹ��
char Memory_En    =0;           //�ֶ�Ԥ��ʹ��

uint32 wandjs = 0;//�������

char straight_flag=0;//ֱ����־λ
char straight_rush_flag=0;//ֱ�������־λ
int straight_count=0;//ֱ�����Ӽ���

int break_flag=0;//���߱�־λ����¼���һ�ζ���

int break2_flag=1;//˫�߱�־λ����¼���һ��˫����

char zhidao_count_jis=0,zhidao_count_jis_old=0;//�ۼ�ֱ��·

char jis_flag=1;//Ԫ������·�α�ʶλ
uint16 jis_plush=0;

uint8  ys_conut=0;//��ͨ����Ԫ������
uint8  ys_sz[20]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

float  cs_yuand[10]={-50,-40,-30,-20,-10,10,20,30,40,50};
//0��Բ����1����·��2���ϰ���3���µ���4��ʮ�֣�5��������

char PID_flag=0;//PID�ٶȻ�����
 //********************���߱���*******************
int Topoint;             //�����ʼֵ
int Top;
int TopFlag;
int TopointFlag;
int leftpoint1;
int rightpoint1;
int leftpoint2;
int rightpoint2;
int R;
int L;
char Flag;
char Flag_towline=0;
int value_old_35=0;
//*******************************˫��ֵ�ų�����*********************************/
int black=60;
int white=110;
int Threshold=0;
int Lost_line_l=0,Lost_line_r=0,repeated_line_r=0,repeated_line_l=0,Lost_line=0;

/************************************��Ⱥ�Ѳ��**********************************************/
#define CBH_YUZHI 15
int cbh_L =0,cbh_R = 0;


float right_cbh(int right_i,int Nhang)
{
    int a=out[Nhang*COL+right_i]+out[Nhang*COL+right_i-1]+out[Nhang*COL+right_i-2];
    int b = out[Nhang*COL+right_i+1]+out[Nhang*COL+right_i+2]+out[Nhang*COL+right_i+3];
    int c = out[(Nhang+1)*COL+right_i-1]+out[(Nhang+1)*COL+right_i]+out[(Nhang+1)*COL+right_i+1];
    int d = out[(Nhang-1)*COL+right_i-1]+out[(Nhang-1)*COL+right_i]+out[(Nhang-1)*COL+right_i+1];
    return ((float)(a-b)/(float)(a+b)+fabs(((float)(d-c)/(float)(d+c))))*100;
}

float left_cbh(int left_i,int Nhang)
{
    int a = out[Nhang*COL+left_i]+out[Nhang*COL+left_i+1]+out[Nhang*COL+left_i+2];
    int b = out[Nhang*COL+left_i-1]+out[Nhang*COL+left_i-2]+out[Nhang*COL+left_i-3];
    int c = out[(Nhang+1)*COL+left_i-1]+out[(Nhang+1)*COL+left_i]+out[(Nhang+1)*COL+left_i+1];
    int d = out[(Nhang-1)*COL+left_i-1]+out[(Nhang-1)*COL+left_i]+out[(Nhang-1)*COL+left_i+1];
    return ((float)(a-b)/(float)(a+b)+fabs((float)(d-c)/(float)(d+c)))*100;
}

//�������
int Track_width[24]=
{
   177-11, 174-13, 172-15, 170-18, 168-21, 165-23, 162-25,
   160-27, 157-31, 155-33, 151-36, 149-38, 147-41, 144-43,
   141-46, 139-49, 137-51, 134-54, 131-57, 129-59, 125-63,
   122-65, 119-67, 117-69
};
//����Ԫ���ж�
int White_average_jia=0,White_average=0,White_average_count=0,Width_average1[24]={0},Width_num=0,Width_count_num=0;//��ɫƽ��ֵ

int no_left=0;
int no_right=0;
int all_out_left_last[24]={0};
int all_out_right_last[24]={0};
//IFX_ALIGN(4) uint8 out3[COL*ROW];

void Camera_Get_Value(void)
{
        float i = 0;
        angle_value=0;
        First_LineFind();
//        new_Ring_LineFind();
//        Garageout_step=2;//**********

//        if(zero.l>0)
//        Garageout_step=2;
//        chuku();           //����
//        yuansujis_count();
//
//        if(Ramp_Flag==0&&GarageOut_En==0&&zhi_flag==0)
//            Threshold_value();
////        if(Cross_Flag==0&&Ramp_Flag==0&&zhi_flag==0&&duanflag==0&&bmx_flag==0&&Ring_Flag==0&&Garageout_step==2)
////            Second_LineFind();//������
//        if(Cross_Flag==0&&Ramp_Flag==0&&zhi_flag==0&&duanflag==0&&bmx_flag==0&&Garageout_step==2&&jis_flag==0)
//            new_Ring_offset(); //Բ��
//        if(Ring_Flag==0&&Ramp_Flag==0&&zhi_flag==0&&duanflag==0&&bmx_flag==0&&Garageout_step==2&&jis_flag==0)
            Cross_Contrl();//ʮ��
//        if(Ring_Flag==0&&Ramp_Flag==0&&zhi_flag==0&&Cross_Flag==0&&bmx_flag==0&&Garageout_step==2&&jis_flag==0)
//            duanlu();      //��·
////        if(Ring_Flag==0&&Cross_Flag==0&&duanflag==0&&bmx_flag==0&&Ramp_Flag==0&&Garageout_step==2&&jis_flag==0&&ForkRoad_En>0&&0)//
////            zhanai();      //�ϰ�
//        if(Ring_Flag==0&&zhi_flag==0&&duanflag==0&&bmx_flag==0&&Cross_Flag==0&&Garageout_step==2&&jis_flag==0&&Ramp_En==1)
//            podao();       //�µ�
//        if(Ring_Flag==0&&zhi_flag==0&&duanflag==0&&Ramp_Flag==0&&Cross_Flag==0&&Garageout_step==2&&jis_flag==0)
//            Destination_Z();//��������Ѱ��
////        if((Cross_Flag==0&&Ramp_Flag==0&&zhi_flag==0&&duanflag==0&&bmx_flag==0&&Ring_Flag==0&&Garageout_step==2))
////             Second_LineFind();//������
//
////        if((Cross_Flag==0&&Ramp_Flag==0&&zhi_flag==0&&duanflag==0&&bmx_flag==0&&Ring_Flag==0&&Garageout_step==2)&&jis_flag==0)
////        straight_line();//ֱ��
////        if(all_out_flag[18]!=4&&all_out_flag[17]!=4&&all_out_flag[16]!=4&&wandjs<10)
////            angle_value_net=value[3]*0.8+all_out_value[16]*0.5+all_out_value[17]*0.25+all_out_value[18]*0.25+all_out_value[14]*0.1;
////        else if(all_out_flag[17]!=4&&all_out_flag[16]!=4&&wandjs<10)
////            angle_value_net=value[3]*0.8+all_out_value[16]*0.5+all_out_value[17]*0.5+all_out_value[14]*0.1;
////        else if(all_out_flag[16]!=4&&wandjs<10)
////            angle_value_net=value[3]*0.8+all_out_value[16]*0.1+all_out_value[14]*0.1;
////        else
////            angle_value_net=value[3]*0.93+all_out_value[14]*0.1;
//
//        if(zhi_value_flag==1)
////            angle_value_net=all_out_value[break2_flag-1]*(1+1/(break2_flag+15));
//        {
//            Speed_Aim_Buff=80;
//            angle_value_net=30;
////            angle_value_net=-35;
//        }
//        else  if(zhi_value_flag==2)
//        {
//            Speed_Aim_Buff=130;//80
//            angle_value_net=0;
//        }
//        else if(zhi_value_flag==3)//�ϰ�
//        {
//            Speed_Aim_Buff=130;//100
//            if(value[3]<-40)
//                value[3]=-40;
////            if(value[3]>40)
////                value[3]=40;
//            angle_value_net=value[3];
//        }
//        else if(duanflag>0)//��·
//        {
//            Speed_Aim_Buff=120; //��duanflag==1��ʱ��������ò����ģ���Ϊ��������ͷƫ�����duanflag==1ʹ�õ��ǵ��Ѱ��
//            if(duanflag==1)
//                angle_value_net=0;
//            if(duanflag==2)
//                angle_value_net=value[3];
//        }
//        else if(Ramp_Flag==1||Ramp_Flag==2)//�µ�
//        {
//            Speed_Aim_Buff=110;//+(AGLE-Balance_Bal_AG_Angle)*2
//            if(Ramp_Flag==1&&value[0]==1)
//                angle_value_net=value[0]*1.2;
//            else if(Ramp_Flag==2)
//                angle_value_net=value[3];
//            angle_value_net=angle_value_net>15?15:angle_value_net;
//            angle_value_net=angle_value_net<-15?-15:angle_value_net;
//        }
//        else if(Garageout_step==0)
//        {
//            Speed_Aim_Buff=80;
//            angle_value_net=0;
//        }
//        else if(Garageout_step==1)//����
//        {
//            Speed_Aim_Buff=80;
//            angle_value_net=zero.r;
//        }
//        else if(Ring_Flag!=0)//Բ��
//        {
//            Speed_Aim_Buff=155;
//            angle_value_net=value[3];
//        }
//        else if((bmx_flag==2||bmx_flag==1))//������
//        {
//            if(bmx_count>=bmx_count_value-1)
//                Speed_Aim_Buff=120;
//            else
//                Speed_Aim_Buff=Speed_Aim;
//
//            angle_value_net=value[3];
//            if(bmx_flag>1){
//                if(bmx_value_flag==0)
//                {
//                    if(bmx_zy_flag==2)
//                        angle_value_net=-80;
//                    if(bmx_zy_flag==1)
//                        angle_value_net=80;
//                }
//    //            angle_value_net=all_out_value[4]-5;
//                else
//                {
//                    if(bmx_zy_flag==2)
//                        angle_value_net=-80;
//                    if(bmx_zy_flag==1)
//                        angle_value_net=80;
//                }
//            }
//            else
//            {
//                if(fabs(value[3])>15)value[3]=(value[3]/fabs(value[3]))*15;
//            }
//
////            angle_value_net=angle_value_net>-20?-30:angle_value_net;
//
//        }
//        else if(straight_rush_flag==1)
//        {
//            Speed_Aim_Buff=Speed_Aim+20;
//            angle_value_net=value[3];
//        }
//        else if(straight_rush_flag==2)
//       {
//           Speed_Aim_Buff=Speed_Aim+break2_flag/3;
//           angle_value_net=value[3];
//       }
////        else if(all_out_flag[18]!=4&&all_out_flag[17]!=4&&all_out_flag[16]!=4)
////        {
////            all_out_value[18]*0.1>5?5:all_out_value[18];
////            all_out_value[18]*0.1<-5?-5:all_out_value[18];
////            all_out_value[17]*0.2>5?5:all_out_value[17];
////            all_out_value[17]*0.2<-5?-5:all_out_value[17];
////            all_out_value[16]*0.3>5?5:all_out_value[16];
////            all_out_value[16]*0.3<-5?-5:all_out_value[16];
////            angle_value_net=value[3]*0.4+all_out_value[16]*0.3+all_out_value[17]*0.2+all_out_value[18]*0.1;
////        }
////        else if(all_out_flag[17]==1&&all_out_flag[16]==1)
////        {
////            all_out_value[17]*0.2>5?5:all_out_value[17];
////            all_out_value[17]*0.2<-5?-5:all_out_value[17];
////            all_out_value[16]*0.2>5?5:all_out_value[16];
////            all_out_value[16]*0.2<-5?-5:all_out_value[16];
////            angle_value_net=value[3]*0.5+all_out_value[16]*0.3+all_out_value[17]*0.2;
////        }
////        else if(all_out_flag[16]==1)
////        {
////            all_out_value[16]*0.3>5?5:all_out_value[16];
////            all_out_value[16]*0.3<-5?-5:all_out_value[16];
////            angle_value_net=value[3]*0.6+all_out_value[16]*0.4;
////        }
//        else{
//            angle_value_net=value[3];


//        if(angle_value_net-angle_value_Old>40&&((angle_value_net>0&&angle_value_Old>0)||(angle_value_net>0&&angle_value_Old<0)))
//            angle_value_net=angle_value_Old+(angle_value_Old-angle_value_Oldd);
//        else if(angle_value_net-angle_value_Old<-40&&((angle_value_net<0&&angle_value_Old<0)||(angle_value_net<0&&angle_value_Old>0)))
//            angle_value_net=angle_value_Old+(angle_value_Old-angle_value_Oldd);
//        }



//        angle_value_net=angle_value_net+(XY_Deviation*XY_Deviation)/16900*(XY_Deviation/fabs(XY_Deviation))*15;

        /* �����˲���� */
//        if(Ring_Flag==6)
//        {
//            angle_value = weightedFiltering(angle_value_zu,value[3]);
//        }
//        else
//        {
//            angle_value = weightedFiltering(angle_value_zu,value[3]);
//        }
//            for(;i<6;i++)
//                angle_value +=  all_out_value[13+i]*(6/(i+1)*0.05);
//            i = fabs(angle_value_zu[0])/600;
            angle_value = all_out_value[13]*0.1+all_out_value[14]*0.2+all_out_value[15]*0.4+all_out_value[16]*0.2+all_out_value[17]*0.05+all_out_value[18]*0.05;
//        if((angle_value-angle_value_Old))
//            angle_value_zu[0] = angle_value;
//        Bee_on();

//        if(fabs(angle_value)>20)
//            wandjs++;
//        else
//            wandjs=0;
//           if(wandjs>20)
//               Bee_off();
//           else
//               Bee_on();
//        angle_value=value[3];
//        if(bmx_flag<2){
         if(angle_value>60)
            angle_value=60;
         if(angle_value<-60)
            angle_value=-60;

        angle_value_Error = angle_value - 2*angle_value_Old + angle_value_Oldd;
        angle_value_Old = angle_value;
        angle_value_Oldd = angle_value_Old;
}
//***********************����Ѱ��***************************

void First_LineFind(void)
{
     char break_flag_count=0;
     int i=0,j=0,Nhang=0;
     int left_dot=0,right_dot=0,left_dot_count=0,right_dot_count=0,White_error=0,White_dot_error=0,White_dot_error_save=0,White_error_save=0;//����ɨ�ߵĵ�ֵ
//     int Width_Worth[24]={0},Width_count[24]={0};
     static  int left_old=0,right_old=0;
     Lost_line_l=0,Lost_line_r=0,repeated_line_r=0,repeated_line_l=0,Lost_line=0,break2_flag=0;
     White_average_count=0,White_average=0;
     break_flag=24;
     line_Init();

//     if(zhidao_count_jis>0)
     zhidao_count_jis_old=0;

     for(j=0;j<=(hang[5]-hang[0])/2+1;j++)
     {


        Nhang=(hang[0]+2*j);


       if(j<=19)                                  //������������������Ͻ�*************************************************
      {   if(j==0&&zhi_flag!=3)
           i=94;             //��������У������ϴε��е�����ε��е㡣
          else if(j==0&&zhi_flag==3)
          {
              i=10;//******************************
          }
          else
                    {
//                  if(right_old>=left_old&&all_out_left[j-1]!=-1){  //�Ž��Ըģ�����ɨ�ߣ����Ż�
//                      left_old=all_out_left[j-1]+10;
//                      i=left_old;}
////                    else    if(right_old>=left_old&&all_out_left[j-1]==-1)
////                    {i=8;left_old=i;}
//                  else    if(all_out_left[j-1]==-1&&all_out_right[j-1]!=-1)
//                  {i=all_out_right[j-1]-10;left_old=i;}
//                  else
//                  {i=all_out_scan_mid[j-1];//���������е��е�������ε��е㣬
//                      left_old=i;}
                        i=all_out_scan_mid[j-1];
                    }

         for(;i>=4;i--)//�����
          {
             if(left_cbh(i,Nhang)>CBH_YUZHI)//���ּ�ⷽ������ȥ�����
//             if(((int)((float)(out[Nhang*COL+i]+out[Nhang*COL+i+1]+
//                            out[Nhang*COL+i+2]-out[Nhang*COL+i-1]-
//                            out[Nhang*COL+i-2]-out[Nhang*COL+i-3])/
//                (float)(out[Nhang*COL+i]+out[Nhang*COL+i+1]+
//                        out[Nhang*COL+i+2]+out[Nhang*COL+i-1]+
//                        out[Nhang*COL+i-2]+out[Nhang*COL+i-3])*100)>6))


            {
                  all_out_left[j]=i;
                  break;
            }
             if(out[Nhang*COL+i]>50){
                 left_dot_count++;           //************************����׵��ƽ��ֵ
                 left_dot+=out[Nhang*COL+i];
             }
           }

        if(j==0)
            i=94;               //��������У������ϴε��е�����ε��е㡣
        else {
//                  if(right_old>=left_old&&all_out_right[j-1]!=-1){
//                      right_old=all_out_right[j-1]-10;
//                      i=right_old;}
////                    else    if(right_old>=left_old&&all_out_right[j-1]==-1)
////                    {i=180;right_old=i;}
//                  else    if(all_out_left[j-1]!=-1&&all_out_right[j-1]==-1)
//                  {i=all_out_left[j-1]+10;right_old=i;}
//                  else
//                  { i=all_out_scan_mid[j-1];//���������е��е�������ε��е㣬
//                   right_old=i;}
                    i=all_out_scan_mid[j-1];
                }
          for(;i<COL-5;i++)//���ұ�
           {
              if(right_cbh(i,Nhang)>CBH_YUZHI)//���ּ�ⷽ������ȥ�����
//              if(((int)((float)(out[Nhang*COL+i]+out[Nhang*COL+i-1]+
//                                          out[Nhang*COL+i-2]-out[Nhang*COL+i+1]-
//                                          out[Nhang*COL+i+2]-out[Nhang*COL+i+3])/
//                              (float)(out[Nhang*COL+i]+out[Nhang*COL+i+1]+
//                                      out[Nhang*COL+i+2]+out[Nhang*COL+i-1]+
//                                      out[Nhang*COL+i-2]+out[Nhang*COL+i-3])*100)>6))
              {
                 all_out_right[j]=i;
                 break;
              }
              if(out[Nhang*COL+i]>50){
                  right_dot_count++;
                  right_dot+=out[Nhang*COL+i];
              }
            }

       }
       else                                //Զ��ģ����********************************************
      {
//         if(right_old>=left_old&&all_out_left[j-1]!=-1){
//                      left_old=all_out_left[j-1]+10;
//                      i=left_old;}
//                  else    if(all_out_left[j-1]==-1&&all_out_right[j-1]!=-1)
//                      i=all_out_right[j-1]-10;
//                  else
           i=all_out_scan_mid[j-1];//���������е��е�������ε��е㣬
//       if(j==0)
//           i=94;
     for(;i>=4;i--)//�����
        {
          if(left_cbh(i,Nhang)>CBH_YUZHI)//���ּ�ⷽ������ȥ�����
//         if(((int)((float)(out[Nhang*COL+i]+out[Nhang*COL+i+1]+
//                                     out[Nhang*COL+i+2]-out[Nhang*COL+i-1]-
//                                     out[Nhang*COL+i-2]-out[Nhang*COL+i-3])/
//                         (float)(out[Nhang*COL+i]+out[Nhang*COL+i+1]+
//                                 out[Nhang*COL+i+2]+out[Nhang*COL+i-1]+
//                                 out[Nhang*COL+i-2]+out[Nhang*COL+i-3])*100)>6))
           {
              all_out_left[j]=i;
              break;
            }
          if(out[Nhang*COL+i]>50){
              left_dot_count++;
              left_dot+=out[Nhang*COL+i];
          }
         }

//      if(right_old>=left_old&&all_out_right[j-1]!=-1){
//                      right_old=all_out_right[j-1]-10;
//                      i=right_old;}
//                  else    if(all_out_left[j-1]!=-1&&all_out_right[j-1]==-1)
//                      i=all_out_left[j-1]+10;
//                  else

            i=all_out_scan_mid[j-1];//���������е��е�������ε��е㣬
     for(;i<COL-5;i++)//���ұ�
      {
          if(right_cbh(i,Nhang)>CBH_YUZHI)//���ּ�ⷽ������ȥ�����
//         if(((int)((float)(out[Nhang*COL+i]+out[Nhang*COL+i-1]+
//                                                   out[Nhang*COL+i-2]-out[Nhang*COL+i+1]-
//                                                   out[Nhang*COL+i+2]-out[Nhang*COL+i+3])/
//                                       (float)(out[Nhang*COL+i]+out[Nhang*COL+i+1]+
//                                               out[Nhang*COL+i+2]+out[Nhang*COL+i-1]+
//                                               out[Nhang*COL+i-2]+out[Nhang*COL+i-3])*100)>6))

          {
            all_out_right[j]=i;
            break;
          }
          if(out[Nhang*COL+i]>50){
              right_dot_count++;
              right_dot+=out[Nhang*COL+i];
          }
       }

      }
       //*********************���⴦������һ��ɨ������ʱ���������ɨ**********************************************
        if(j==0&&all_out_right[j]==-1&&all_out_left[j]==-1)
        {
            if((((value[3]<0||value_old_35<0)&&zhi_flag==0)||(out[35*COL+180])>Threshold-30))
            {
                i=176;
                for(;i>=4&&i<COL-2;i--)//�����
                    {
                        if(left_cbh(i,Nhang)>CBH_YUZHI)//���ּ�ⷽ������ȥ�����
                         {
                                all_out_left[j]=i;
                                break;
                            }
                     }
//                Bee_on();
             }
            else if((((value[3]>0||value_old_35>0)&&zhi_flag==0)||(out[35*COL+7])>Threshold-30))//�����ֹ����
            {
                i=9;
                 for(;i<COL-5&&i>2;i++)//���ұ�
                {
          if(right_cbh(i,Nhang)>CBH_YUZHI)//���ּ�ⷽ������ȥ�����
          {
            all_out_right[j]=i;
            break;
          }
//          Bee_on();

       }
         }
        }
//      if(Cross_Flag!=0){
            if((all_out_left[j-1]>-1&&all_out_left[j]==-1&&j>0)||(all_out_left[22]>-1&&all_out_left[23]>-1&&Lost_line_l==0))
                Lost_line_l=j;//����
            if((all_out_right[j-1]>-1&&all_out_right[j]==-1&&j>0)||(all_out_right[22]>-1&&all_out_right[23]>-1&&Lost_line_l==0))
                Lost_line_r=j;
            if(Lost_line_l>0&&all_out_left[j-1]==-1&&all_out_left[j]>-1&&j>0)
                repeated_line_l=j;//��
            if(Lost_line_r>0&&all_out_right[j-1]==-1&&all_out_right[j]>-1&&j>0)
                repeated_line_r=j;

            if(((repeated_line_l-Lost_line_l<3&&repeated_line_l>0)||((repeated_line_r-Lost_line_r<3&&repeated_line_r>0)))&&j>0)
                Lost_line=1;//���߸��߱�־λ

//      }
    //����ƫ�������е��������´����е�
      if(all_out_left[j]>-1&&all_out_right[j]>-1){                //�����߶�ɨ��
      all_out_flag[j]=1;
      all_out_scan_mid[j]=(all_out_left[j]+all_out_right[j])/2;
      all_out_value[j]=COL/2-all_out_scan_mid[j];
      all_out_edge_Width[j]=all_out_right[j]-all_out_left[j];           //�������ߵĿ��
      if(break_flag_count==0)
          break2_flag=j;
//      Width_Worth[j]+=all_out_edge_Width[j];
//      Width_count[j]++;
    }

    if(all_out_left[j]>-1&&all_out_right[j]==-1){              //���ɨ��
      all_out_flag[j]=2;

//      if(left_bz[j]>all_out_left[j])
//      {
//          left_bz[j]=all_out_left[j];  //ʵ����϶�������
//
//      }
      all_out_edge_Width[j]=COL-all_out_left[j];
      if(j>4)
      break_flag_count=1;
//      all_out_scan_mid[j]=(2*all_out_left[j]+all_out_Width)//2����ֵ���Ͽ��
//      all_out_scan_mid[j]=(2*all_out_right[j]-Track_width[j])
      all_out_scan_mid[j]=(all_out_left[j]+COL)/2;        //ɨ�����ߣ������һ�εĿ�Ⱥͱߵ�λ�ü�����е�//+(int)(-left_bz[j]+all_out_left[j])/3
//      all_out_scan_mid[j]=(all_out_left[j]-j/3+COL)/2;//2����ֵ���Ͽ��
      all_out_value[j]=COL/2-all_out_scan_mid[j];         //���ɨ������������ͷ�������ĵ�ƫ����
//          all_out_edge_Width[j]=188-all_out_left[j];
    }
    else
        left_bz[j]=0;

    if(all_out_right[j]>-1&&all_out_left[j]==-1){             //�ұ�ɨ��
      all_out_flag[j]=3;

      all_out_edge_Width[j]=all_out_right[j];
//      if(right_bz[j]<all_out_right[j])
//            {
//                  right_bz[j]=all_out_right[j];  //ʵ����϶�������
//            }
      if(j>4)
      break_flag_count=1;
      all_out_scan_mid[j]=(all_out_right[j])/2;       //ɨ�����ߣ������һ�εĿ�Ⱥͱߵ�λ�ü�����е�//+(int)(all_out_right-right_bz[j])/3
//      all_out_scan_mid[j]=(all_out_right[j]-j/3)/2;
      all_out_value[j]=COL/2-all_out_scan_mid[j];        //���ɨ������������ͷ�������ĵ�ƫ����
//          all_out_edge_Width[j]=all_out_right[j];
    }
    else
        right_bz[j]=0;

    if(all_out_right[j]==-1&&all_out_left[j]==-1){           //��û��
      all_out_flag[j]=4;

      if(j>4)
      break_flag_count=1;

      White_error+=(left_dot+right_dot-White_average);//������һ���ܵİ�ɫֵ
      White_dot_error+=(left_dot_count+right_dot_count-White_average_count);

//      oled_show_int(42, 2, White_average, 4);
      break_flag=j<break_flag?j:break_flag;//�ж�����ߵ�λ��

      if(j>0)
      all_out_scan_mid[j]=all_out_scan_mid[j-1];
      else
        all_out_scan_mid[j]=94;


      if(j>4)
      all_out_value[j]=all_out_value[j-1]+((all_out_value[break_flag-1]-all_out_value[break_flag-2])/2)*(j*break_flag-1);//��ǰ����û��

    }

    White_average=left_dot+right_dot;                    //������ʵ��ɫ���
    White_average_count=left_dot_count+right_dot_count;


    if(all_out_right[j]<all_out_right[j-1]+2&&j>0&&all_out_right[j]>0&&all_out_right[j-1]>0)
        zhidao_count_jis_old++;
    if(all_out_left[j]>all_out_left[j-1]-2&&j>0&&all_out_left[j]>0&&all_out_left[j-1]>0)
        zhidao_count_jis_old++;

        if(j>14)//���⴦����15��ɨ�����Ǻڣ�ֱ���˳�
             {
                 if((int)(out[Nhang*COL+all_out_scan_mid[j]]+out[Nhang*COL+all_out_scan_mid[j]-1]+out[Nhang*COL+all_out_scan_mid[j]-2]
                    +out[Nhang*COL+all_out_scan_mid[j]+1]+out[Nhang*COL+all_out_scan_mid[j]+2]+out[Nhang*COL+all_out_scan_mid[j]+3])/6<35)
                 {
                     break_flag=j<break_flag?j:break_flag;

                     break;
                 }
             }
   }
//     oled_show_int(44, 2, (int) all_out_left[15], 4);
//     oled_show_int(0, 2, (int) all_out_left[16], 4);
//     oled_show_int(44, 4, (int) all_out_right[15], 4);
//     oled_show_int(0, 4, (int) all_out_right[16], 4);

     White_average_jia=(int)((left_dot+right_dot)/(left_dot_count+right_dot_count)); //*****ɨ��ƽ��ֵ
     if(White_average_count-White_dot_error>0)
     White_average=(int)((White_average-White_error)/(White_average_count-White_dot_error));//***��ɫƽ��ֵ
     else
     White_average=0;

     Width_count_num=left_dot_count+right_dot_count;//�����ɫ��ĸ���

     zhidao_count_jis=zhidao_count_jis_old;
//     oled_show_int(42, 2, White_average, 8);
    /**��all_outϵ�б����л�ȡ���õ�5��հ*****/
   for(i=0;i<6;i++)
  {
     j=(hang[i]-hang[0])/2;//5��հ��all_out�����ж�Ӧ��λ��
     flag[i]=all_out_flag[j];
     scan_mid[i]=all_out_scan_mid[j];
     value[i]=all_out_value[j];
     edge_Width[i]=all_out_edge_Width[j];
     right[i]=all_out_right[j];
     left[i]=all_out_left[j];
  }
    value_old_35=all_out_value[0];
}  //�����Ļ�����
//void First_LineFind(void)A
//{
//     int i=0,j=0,Nhang=0;
//            static  int left_old=0,right_old=0;
//     Lost_line_l=0,Lost_line_r=0,repeated_line_r=0,repeated_line_l=0,Lost_line=0;
//     line_Init();
//     for(j=0;j<=(hang[5]-hang[0])/2+1;j++)
//     {
//
//
//        Nhang=(hang[0]+2*j);
//
//
//       if(j<=19)                                  //������������������Ͻ�*************************************************
//      {   if(j==0)
//           i=94;             //��������У������ϴε��е�����ε��е㡣
//          else
//                    {
////                  if(right_old>=left_old&&all_out_left[j-1]!=-1){  //�Ž��Ըģ�����ɨ�ߣ����Ż�
////                      left_old=all_out_left[j-1]+10;
////                      i=left_old;}
//////                    else    if(right_old>=left_old&&all_out_left[j-1]==-1)
//////                    {i=8;left_old=i;}
////                  else    if(all_out_left[j-1]==-1&&all_out_right[j-1]!=-1)
////                  {i=all_out_right[j-1]-10;left_old=i;}
////                  else
////                  {i=all_out_scan_mid[j-1];//���������е��е�������ε��е㣬
////                      left_old=i;}
//                        i=all_out_scan_mid[j-1];
//                    }
//
//         for(;i>=3;i--)//�����
//          {   if((out[Nhang*COL+i]+out[Nhang*COL+i+1]+
//                 out[Nhang*COL+i+2]-out[Nhang*COL+i-1]-
//                 out[Nhang*COL+i-2]-out[Nhang*COL+i-3]>45)
//                 &&i<180&&out[Nhang*COL+i-1]+out[Nhang*COL+i-3]<2*black&&out[Nhang*COL+i+2]
//                 +out[Nhang*COL+i]<2*white)//���ּ�ⷽ������ȥ�����
//
//
//            {
//                  all_out_left[j]=i;
//                  break;
//            }
//           }
//
//        if(j==0)
//            i=94;               //��������У������ϴε��е�����ε��е㡣
//        else {
////                  if(right_old>=left_old&&all_out_right[j-1]!=-1){
////                      right_old=all_out_right[j-1]-10;
////                      i=right_old;}
//////                    else    if(right_old>=left_old&&all_out_right[j-1]==-1)
//////                    {i=180;right_old=i;}
////                  else    if(all_out_left[j-1]!=-1&&all_out_right[j-1]==-1)
////                  {i=all_out_left[j-1]+10;right_old=i;}
////                  else
////                  { i=all_out_scan_mid[j-1];//���������е��е�������ε��е㣬
////                   right_old=i;}
//                    i=all_out_scan_mid[j-1];
//                }
//          for(;i<COL-3;i++)//���ұ�
//           {
//              if((out[Nhang*COL+i]+out[Nhang*COL+i-1]+
//              out[Nhang*COL+i-2]-out[Nhang*COL+i+1]-
//              out[Nhang*COL+i+2]-out[Nhang*COL+i+3]>45)
//                 &&i>7&&out[Nhang*COL+i+3]+out[Nhang*COL+i+1]<2*black&&out[Nhang*COL+i-2]
//                   +out[Nhang*COL+i]<2*white)//���ּ�ⷽ������ȥ�����
//              {
//                 all_out_right[j]=i;
//                 break;
//              }
//            }
//
//       }
//       else                                //Զ��ģ����********************************************
//      {
////         if(right_old>=left_old&&all_out_left[j-1]!=-1){
////                      left_old=all_out_left[j-1]+10;
////                      i=left_old;}
////                  else    if(all_out_left[j-1]==-1&&all_out_right[j-1]!=-1)
////                      i=all_out_right[j-1]-10;
////                  else
//           i=all_out_scan_mid[j-1];//���������е��е�������ε��е㣬
//     for(;i>=3;i--)//�����
//        {
//          if((out[Nhang*COL+i]+out[Nhang*COL+i+1]+
//         out[Nhang*COL+i+2]-out[Nhang*COL+i-1]-
//         out[Nhang*COL+i-2]-out[Nhang*COL+i-3]>30)
//         &&i<180&&out[Nhang*COL+i-1]+out[Nhang*COL+i-3]<2*black+30&&out[Nhang*COL+i+2]
//           +out[Nhang*COL+i]<2*white)//���ּ�ⷽ������ȥ�����
//           {
//              all_out_left[j]=i;
//              break;
//            }
//         }
//
////      if(right_old>=left_old&&all_out_right[j-1]!=-1){
////                      right_old=all_out_right[j-1]-10;
////                      i=right_old;}
////                  else    if(all_out_left[j-1]!=-1&&all_out_right[j-1]==-1)
////                      i=all_out_left[j-1]+10;
////                  else
//            i=all_out_scan_mid[j-1];//���������е��е�������ε��е㣬
//     for(;i<COL-3;i++)//���ұ�
//      {
//          if((out[Nhang*COL+i]+out[Nhang*COL+i-1]+
//         out[Nhang*COL+i-2]-out[Nhang*COL+i+1]-
//         out[Nhang*COL+i+2]-out[Nhang*COL+i+3]>30)
//         &&i>7&&out[Nhang*COL+i+3]+out[Nhang*COL+i+1]<2*black+30&&out[Nhang*COL+i-2]
//           +out[Nhang*COL+i]<2*white)//���ּ�ⷽ������ȥ�����
//          {
//            all_out_right[j]=i;
//            break;
//          }
//
//       }
//
//      }
//        if(j==0&&all_out_right[j]==-1&&all_out_left[j]==-1)//���⴦������һ��ɨ������ʱ���������ɨ
//        {
//            if(value[3]>0||value_old_35>0)
//            {
//                i=176;
//                for(;i>=3;i--)//�����
//                    {
//                        if((out[Nhang*COL+i]+out[Nhang*COL+i+1]+
//                     out[Nhang*COL+i+2]-out[Nhang*COL+i-1]-
//                     out[Nhang*COL+i-2]-out[Nhang*COL+i-3]>30)
//                     &&i<180&&out[Nhang*COL+i-1]+out[Nhang*COL+i-3]<2*black+30&&out[Nhang*COL+i+2]
//                         +out[Nhang*COL+i]<2*white)//���ּ�ⷽ������ȥ�����
//                         {
//                                all_out_left[j]=i;
//                                break;
//                            }
//                     }
//             }
//            if(value[3]<0||value_old_35<0)//�����ֹ����
//            {
//                i=9;
//                 for(;i<COL-3;i++)//���ұ�
//                {
//          if((out[Nhang*COL+i]+out[Nhang*COL+i-1]+
//         out[Nhang*COL+i-2]-out[Nhang*COL+i+1]-
//         out[Nhang*COL+i+2]-out[Nhang*COL+i+3]>30)
//         &&i>7&&out[Nhang*COL+i+3]+out[Nhang*COL+i+1]<2*black+30&&out[Nhang*COL+i-2]
//           +out[Nhang*COL+i]<2*white)//���ּ�ⷽ������ȥ�����
//          {
//            all_out_right[j]=i;
//            break;
//          }
//
//       }
//         }
//        }
////      if(Cross_Flag!=0){
//            if(all_out_left[j-1]>-1&&all_out_left[j]==-1&&j>0)
//                Lost_line_l=j;
//            if(all_out_right[j-1]>-1&&all_out_right[j]==-1&&j>0)
//                Lost_line_r=j;
//            if(Lost_line_l>0&&all_out_left[j-1]==-1&&all_out_left[j]>-1&&j>0)
//                repeated_line_l=j;
//            if(Lost_line_r>0&&all_out_right[j-1]==-1&&all_out_right[j]>-1&&j>0)
//                repeated_line_r=j;
//
//            if(((repeated_line_l-Lost_line_l<3&&repeated_line_l>0)||((repeated_line_r-Lost_line_r<3&&repeated_line_r>0)))&&j>0)
//                Lost_line=1;//���߸��߱�־λ
//
////      }
//    //����ƫ�������е��������´����е�
//    if(all_out_left[j]>-1&&all_out_right[j]>-1){                //�����߶�ɨ��
//      all_out_flag[j]=1;
//      all_out_scan_mid[j]=(all_out_left[j]+all_out_right[j])/2;
//      all_out_value[j]=COL/2-all_out_scan_mid[j];
//      all_out_edge_Width[j]=all_out_right[j]-all_out_left[j];           //�������ߵĿ��
//    }
//
//    if(all_out_left[j]>-1&&all_out_right[j]==-1){              //���ɨ��
//      all_out_flag[j]=2;
//      all_out_scan_mid[j]=(all_out_left[j]+COL)/2;        //ɨ�����ߣ������һ�εĿ�Ⱥͱߵ�λ�ü�����е�
//      all_out_value[j]=COL/2-all_out_scan_mid[j];         //���ɨ������������ͷ�������ĵ�ƫ����
////          all_out_edge_Width[j]=188-all_out_left[j];
//    }
//
//    if(all_out_right[j]>-1&&all_out_left[j]==-1){             //�ұ�ɨ��
//      all_out_flag[j]=3;
//      all_out_scan_mid[j]=(all_out_right[j])/2;       //ɨ�����ߣ������һ�εĿ�Ⱥͱߵ�λ�ü�����е�
//      all_out_value[j]=COL/2-all_out_scan_mid[j];        //���ɨ������������ͷ�������ĵ�ƫ����
////          all_out_edge_Width[j]=all_out_right[j];
//    }
//
//    if(all_out_right[j]==-1&&all_out_left[j]==-1){           //��û��
//      all_out_flag[j]=4;
//      if(j>0)
//      all_out_scan_mid[j]=all_out_scan_mid[j-1];
//      else
//        all_out_scan_mid[j]=94;
//
//    }
//
//        if(j>14)//���⴦����15��ɨ�����Ǻڣ�ֱ���˳�
//             {
//                 if((int)(out[Nhang*COL+all_out_scan_mid[j]]+out[Nhang*COL+all_out_scan_mid[j]-1]+out[Nhang*COL+all_out_scan_mid[j]-2]
//                    +out[Nhang*COL+all_out_scan_mid[j]+1]+out[Nhang*COL+all_out_scan_mid[j]+2]+out[Nhang*COL+all_out_scan_mid[j]+3])/6<35)
//                 break;
//             }
//
//
//
//
//   }
//
//    /**��all_outϵ�б����л�ȡ���õ�5��հ*****/
//   for(i=0;i<6;i++)
//  {
//     j=(hang[i]-hang[0])/2;//5��հ��all_out�����ж�Ӧ��λ��
//     flag[i]=all_out_flag[j];
//     scan_mid[i]=all_out_scan_mid[j];
//     value[i]=all_out_value[j];
//     edge_Width[i]=all_out_edge_Width[j];
//     right[i]=all_out_right[j];
//     left[i]=all_out_left[j];
//  }
//    value_old_35=all_out_value[0];
//}  //�����Ļ�����
////***********************ȷ����ֵ***************************
void Threshold_value(void)
{
  uint8 i=0;
  static int j=0,Threshold_buf;
  Threshold_buf=Threshold;
  Threshold=0;
  for(i=12;i<=18;i++) //12 13 14 15 16 17 18
  {                                     //24 26 28 30 32 34 36
     if(all_out_left[i]>0&&all_out_left[i]>15)//59 61 63 65 67 69 71
     {
       Threshold=out[(35+i*2)*COL+all_out_left[i]-2]+Threshold;
       j++;
     }
     if(all_out_right[i]>0&&all_out_right[i]<173)
     {
       Threshold=out[(35+i*2)*COL+all_out_right[i]+2]+Threshold;
       j++;
     }
  }
  if(j>6)
  {
    Threshold=Threshold/j;
    if(Threshold<35)
      Threshold=35;
    j=0;
  }
  else
  {
    Threshold=Threshold_buf;
    j=0;
  }
}
void Second_LineFind(void)
{
    int i=0,j=0;
    Topoint=0;//�����ʼֵ
    TopFlag=0;
    TopointFlag=0;
    leftpoint1=0;
    rightpoint1=0;

      for(i=5;i<=23;i++)
    {
      if(TopointFlag==0)//����
      {
        if((out[(i*2+35)*COL+all_out_scan_mid[i]] + out[(i*2+34)*COL+all_out_scan_mid[i]]+
       out[(i*2+33)*COL+all_out_scan_mid[i]]-out[(i*2+36)*COL+all_out_scan_mid[i]]-
       out[(i*2+37)*COL+all_out_scan_mid[i]]-out[(i*2+38)*COL+all_out_scan_mid[i]]>30
         &&out[(i*2+35)*COL+all_out_scan_mid[i]]+out[(i*2+34)*COL+all_out_scan_mid[i]]<2*white
         &&out[(i*2+37)*COL+all_out_scan_mid[i]]+out[(i*2+38)*COL+all_out_scan_mid[i]]<2*black)||
           (out[(i*2+35)*COL+all_out_scan_mid[i]] + out[(i*2+36)*COL+all_out_scan_mid[i]]+
       out[(i*2+37)*COL+all_out_scan_mid[i]]-out[(i*2+38)*COL+all_out_scan_mid[i]]-
       out[(i*2+39)*COL+all_out_scan_mid[i]]-out[(i*2+40)*COL+all_out_scan_mid[i]]>30
         &&out[(i*2+35)*COL+all_out_scan_mid[i]]+out[(i*2+37)*COL+all_out_scan_mid[i]]<2*white
         &&out[(i*2+38)*COL+all_out_scan_mid[i]]+out[(i*2+40)*COL+all_out_scan_mid[i]]<2*black))
        {
         Topoint=i;
        TopointFlag=1;//�ҵ�����
        }
      }
      else
      {
      break;
       }
    }
      for(i=0;i<23;i++)
    {
      if(TopointFlag==0)
      {
        if(all_out_scan_mid[i]<15||all_out_scan_mid[i]>172)
        {
           Topoint=i;
        TopointFlag=1;
        }

      }
      else
      {
        break;
      }
    }
      if(Topoint!=0)
    {
      for(i=5;i<Topoint;i++)
      {
        if(all_out_left[i]==-1)
        {
          if(leftpoint1==0&&rightpoint1==0&&all_out_left[i-1]!=-1&&all_out_left[i-2]!=-1)
          {
            leftpoint1=1;//�����־��
            leftpoint2=i;
            L=Topoint;
            for(i=leftpoint2+5;i<Topoint;i++)
            {
              if(all_out_right[i]==-1)
              {
                leftpoint1=3;
                break;
              }
            }
          }
        }
        if(all_out_right[i]==-1)
        {
            if(leftpoint1==0&&rightpoint1==0&&all_out_right[i-1]!=-1&&all_out_right[i-2]!=-1)
          {
            rightpoint1=1;//�����־��
            rightpoint2=i;
            R=Topoint;
            for(i=rightpoint2+5;i<Topoint;i++)
            {
              if(all_out_left[i]==-1)
              {
                rightpoint1=3;
                break;
              }
            }
          }
        }
      }
    L_big();
    R_big();

    }
       if(leftpoint1==1)//������
    {
      for(i=L;i<23;i++)
      {
        all_out_left[i]=0;
        all_out_right[i]=0;
        all_out_flag[i]=1;
        all_out_scan_mid[i]=0;

      }

      for(i=leftpoint2;i<L&&i>0;i++)
      {
        if(all_out_right[i]>=180)
        {
          all_out_scan_mid[i]=all_out_scan_mid[i-1]-2;

        }
        else
        {
          if(2*all_out_scan_mid[i-1]-all_out_right[i-1]<3)
             all_out_scan_mid[i]=all_out_right[i]/2;
          else
          all_out_scan_mid[i]=(2*all_out_scan_mid[i-1]-all_out_right[i-1]-3+all_out_right[i])/2;

        }
        if(all_out_scan_mid[i]<0)
          all_out_scan_mid[i]=0;
      }
    }
       if(leftpoint1==2)
    {
      for(i=3;i<L;i++)
      {
       all_out_scan_mid[i]=(all_out_right[i])/2;
        if(all_out_scan_mid[i]<0)
        {
            all_out_scan_mid[i]=0;
            L=i;
              break;
        }

      }
      for(i=L;i<23;i++)
      {
        all_out_left[i]=0;
        all_out_right[i]=0;
        all_out_flag[i]=1;
        all_out_scan_mid[i]=0;
      }
    }
       if(rightpoint1==1)
    {
      for(i=R;i<23;i++)
      {
        all_out_left[i]=187;
        all_out_right[i]=187;
        all_out_flag[i]=1;
        all_out_scan_mid[i]=187;


      }
      for(i=rightpoint2;i<R;i++)
      {
        if(all_out_left[i]<7)
        {
          all_out_scan_mid[i]=all_out_scan_mid[i-1]+2;
        }
        else
        {
          if(2*all_out_scan_mid[i-1]-all_out_left[i-1]>184)
            all_out_scan_mid[i]=(all_out_left[i]+187)/2;
          else
          all_out_scan_mid[i]=(2*all_out_scan_mid[i-1]-all_out_left[i-1]+3+all_out_left[i])/2;
        }
        if(all_out_scan_mid[i]>187)
          all_out_scan_mid[i]=187;
      }

    }
       if(rightpoint1==2)
    {
      for(i=3;i<R;i++)
      {
        all_out_scan_mid[i]=(all_out_left[i]+187)/2;
        if(all_out_scan_mid[i]>186)
        {
            all_out_scan_mid[i]=186;
            R=i;
              break;
        }

      }
      for(i=R;i<23;i++)
      {
        all_out_left[i]=186;
        all_out_right[i]=186;
        all_out_flag[i]=1;
        all_out_scan_mid[i]=186;
      }


    }
       for(i=0;i<=23;i++)
    {
           all_out_value[i]=COL/2-all_out_scan_mid[i];

    }
        for(i=1;i<23;i++)
    {
      if(TopFlag==0)
    {
      if(out[(i*2+35)*COL+all_out_scan_mid[i]] + out[(i*2+34)*COL+all_out_scan_mid[i]]+
       out[(i*2+33)*COL+all_out_scan_mid[i]]-out[(i*2+36)*COL+all_out_scan_mid[i]]-
       out[(i*2+37)*COL+all_out_scan_mid[i]]-out[(i*2+38)*COL+all_out_scan_mid[i]]>50
         &&out[(i*2+35)*COL+all_out_scan_mid[i]]+out[(i*2+33)*COL+all_out_scan_mid[i]]<2*white
         &&out[(i*2+38)*COL+all_out_scan_mid[i]]+out[(i*2+36)*COL+all_out_scan_mid[i]]<2*black)
      {
        Top=i;
        TopFlag=1;
      }
    }
    else
    {
    break;
    }
    }
    /**��all_outϵ�б����л�ȡ���õ�5��հ*****/
    for(i=0;i<6;i++)
    {
           j=(hang[i]-hang[0])/2;//5��հ��all_out�����ж�Ӧ��λ��
           flag[i]=all_out_flag[j];
           scan_mid[i]=all_out_scan_mid[j];
           value[i]=all_out_value[j];
           edge_Width[i]=all_out_edge_Width[j];
           right[i]=all_out_right[j];
           left[i]=all_out_left[j];
    }
}//�����Ļ�����
//*********************line��ʼ��***************************
void line_Init(void)
{
    int i=0;
    for(i=0;i<6;i++)
    {
        left[i]=-1;
        right[i]=-1;
        scan_mid[i]=94;
        edge_Width[i]=0;
    }
    for(i=0;i<24;i++)
    {
        all_out_left[i]=-1;
        all_out_right[i]=-1;
        all_out_scan_mid[i]=94;
        all_out_edge_Width[i]=0;
        all_out_flag[i]=0;
    }


}//����������
//***********************���⴦��***************************
//void Garage_Out(void)
//{
//            if(Garageout_flag == 0)
//            {
//                if(GOut_Mode==1)        //1Ϊ��߳���
//                {
//                    Garage_Out_Z();
//                }
//                if(GOut_Mode==2)              //2Ϊ�ұ߳���
//                {
//                    Garage_Out_Y();
//                }
//          }
//            if(Garageout_flag == 0&&((GarageEncoder>40000)||(Garage_Roll<-80&&GOut_Mode==1)||(Garage_Roll>80&&GOut_Mode==2)))
//            {
//                Garageout_flag = 1;
//        GarageEncoder_Flag = 0;
//        GarageEncoder = 0;
//                GarageOut_En = 0;
//                Garage_Roll=0;
//                dm_offset_flag=1;
//                Bee_off();
//            }
//}
/********************************************����������ɨ��(��)***************************************************************/
void Destination_LineFind_Z(void)
{
                right[3]=-1;
          for(int i=COL-4;i>94;i--)//���ұ�
          {
                            if((out[65*COL+i]+out[65*COL+i-1]+
              out[65*COL+i-2]-out[65*COL+i+1]-
              out[65*COL+i+2]-out[65*COL+i+3]>45)
                 &&i<180&&out[65*COL+i]+out[65*COL+i+2]<2*black&&out[65*COL+i-1]
                 +out[65*COL+i-3]<2*white)//���ּ�ⷽ������ȥ�����
            {
                  right[3]=i;
                  break;
            }
                    }

         if(right[3]>-1)//���ɨ��
         {

           flag[3]=2;
                   scan_mid[3]=right[3]-44;
           value[3]=COL/2-scan_mid[3];

          }
                 else
                 {
                     value[3]=0;
                 }

}
/********************************************����������ɨ��(��)***************************************************************/
void Destination_LineFind_Y(void)
{
                left[3]=-1;
          for(int i=4;i<94;i++) //�����
          {   if((out[65*COL+i]+out[65*COL+i+1]+
                 out[65*COL+i+2]-out[65*COL+i-1]-
                 out[65*COL+i-2]-out[65*COL+i-3]>45)
                 &&i<180&&out[65*COL+i-1]+out[65*COL+i-3]<2*black&&out[65*COL+i]
                 +out[65*COL+i+2]<2*white)//���ּ�ⷽ������ȥ�����
            {
                  left[3]=i;
                  break;
            }
                    }
         if(left[3]>-1)//���ɨ��
         {

           flag[3]=2;
                   scan_mid[3]=left[3]+49;
           value[3]=COL/2-scan_mid[3];

          }
         else if(all_out_flag[10]==1&&all_out_right[10]<all_out_right[7])
         {
             value[3]=all_out_value[10]*1.2;
         }
         else
         {
             value[3]=0;
         }
}
void Cross_Contrl(void)
{
 int i=0,j=0,jix=0;
 static int value_old_s=0;
// oled_show_int(0, 0, value[0], 4);
//       oled_show_int(0, 2, value[1], 4);
//       oled_show_int(42,0, value[2], 4);
//       oled_show_int(42, 2, value[3], 4);
//       oled_show_int(84, 0, value[4], 4);
//       oled_show_int(84, 2, value[5], 4);

      if((((flag[3]==4&&all_out_flag[16]==4&&all_out_flag[17]==4)
                ||(flag[3]==2&&left[3]>all_out_left[16]&&all_out_flag[16]==2)
            ||(flag[3]==3&&right[3]<all_out_right[16]&&all_out_flag[16]==3))
            ||(all_out_flag[16]==2&&all_out_left[16]>all_out_left[17]&&all_out_flag[17]==2)
            ||(all_out_flag[16]==3&&all_out_right[16]<all_out_right[17]&&all_out_flag[17]==3)
            ||(Lost_line_l>0&&Lost_line_l<20&&Lost_line_r>0&&Lost_line_r<20))&&Cross_Flag==0&&Lost_line==0)
    {
//          for(i=35;i<=78;i++)
//             {
////              Bee_on();
//               if(((int)((float)(out[i*COL+all_out_scan_mid[0]]+out[(i-1)*COL+all_out_scan_mid[0]]+
//                out[(i-2)*COL+all_out_scan_mid[0]]-out[(i+1)*COL+all_out_scan_mid[0]]-
//                out[(i+2)*COL+all_out_scan_mid[0]]-out[(i+3)*COL+all_out_scan_mid[0]])/
//    (float)(out[i*COL+all_out_scan_mid[0]]+out[(i-1)*COL+all_out_scan_mid[0]]+
//                out[(i-2)*COL+all_out_scan_mid[0]]+out[(i+1)*COL+all_out_scan_mid[0]]+
//                out[(i+2)*COL+all_out_scan_mid[0]]+out[(i+3)*COL+all_out_scan_mid[0]])*100)>6)
//                         &&out[(i+1)*COL+all_out_scan_mid[0]]+out[(i+3)*COL+all_out_scan_mid[0]]<2*black+20
//                         &&out[(i)*COL+all_out_scan_mid[0]]+out[(i-2)*COL+all_out_scan_mid[0]]<2*white)
//                break;
//             }

//             if(i>77||Width_count_num>2600)
              if(Width_count_num>2400)
             {
                             for(i=10;i<24;i++)
                             {
                                 if(all_out_flag[i]==4)
                                     jix++;
                                 else
                                     jix=0;
                                 if(jix>4)
                                     break;
                             }
                         }
             if(jix>4)
             {
               Cross_Flag=1;
               Bee_on();
//               if(value[3]<-8) //��ת��ʮ��
//               {
//               flag_right=1;
//               Cross_old=value[3];
//               if(Cross_old<-10)
//                 Cross_old=-10;
//               }
//               else if(value[3]>8)//��ת
//               {
//               flag_left=1;
//               Cross_old=value[3];
//               if(Cross_old>10)
//                 Cross_old=10;
//               }
             }

    }
   if(Cross_Flag==1)//��ʮ�ֽ��д���
     {
       for(j=0;j<6;j++){//ʮ���ұ߷���
         left[j]=-1;
         right[j]=-1;
         if(flag[0]==4)
         {
           scan_mid[0]=94;
           value[0]=0;
         }
        for(i=184;i>=3;i--){//�����
        if(out[hang[j]*COL+i]+out[hang[j]*COL+i+1]+
        out[hang[j]*COL+i+2]-out[hang[j]*COL+i-1]-
        out[hang[j]*COL+i-2]-out[hang[j]*COL+i-3]>40){//���ּ�ⷽ������ȥ�����
            left[j]=i;
            break;

          }

        }
   for(i=3;i<COL-3;i++){//���ұ�
      if(out[hang[j]*COL+i]+out[hang[j]*COL+i-1]+
         out[hang[j]*COL+i-2]-out[hang[j]*COL+i+1]-
         out[hang[j]*COL+i+2]-out[hang[j]*COL+i+3]>40){//���ּ�ⷽ������ȥ�����
        right[j]=i;
        break;
      }
     }
  }


    for(j=0;j<6;j++){
      if(left[j]>-1&&right[j]>-1){                //�����߶�ɨ��
        flag[j]=1;
        scan_mid[j]=(left[j]+right[j])/2;
        value[j]=COL/2-scan_mid[j];
        edge_Width[j]=right[j]-left[j];           //�������ߵĿ��
        }
      if(left[j]>-1&&right[j]==-1){              //���ɨ��
        flag[j]=2;
        scan_mid[j]=(COL+left[j])/2;        //ɨ�����ߣ�����ȡ����iֵ
        value[j]=COL/2-scan_mid[j];         //���ɨ������������ͷ�������ĵ�ƫ����
        }
      if(right[j]>-1&&left[j]==-1){             //�ұ�ɨ��
        flag[j]=3;
        scan_mid[j]=right[j]/2;                 //ɨ�����ߣ�����ȡ��С��iֵ
        value[j]=COL/2-scan_mid[j];        //���ɨ������������ͷ�������ĵ�ƫ����
        }
      if(left[j]==-1&&right[j]==-1){           //��û��
        flag[j]=4;
        }
//
  }

  if(flag[4]==1&&edge_Width[5]>100)
    flag[4]=4;
  if(flag[5]==1&&edge_Width[5]>100)
  {
    flag[5]=4;
  }
  if(flag[2]!=4&&flag[3]==1&&Width_count_num<2600)
  {
        Cross_Flag=0;
        Bee_off();
        Cross_old=0;
        dm_offset_flag=1;
        Ring_En=0;
//      Cross_En=0;
//        ForkRoad_En=1;
        Ramp_En=1;
        yuansujis();//����·

  }
   else
      {
//          if(Lost_line_l>0&&repeated_line_l>0&&repeated_line_r>0&&Lost_line_r>0)
//              {
//                  left[3]=linear(all_out_left[repeated_line_l],35+2*repeated_line_l,all_out_right[Lost_line_l],35+2*Lost_line_l,65);
//                  right[3]=linear(all_out_left[repeated_line_r],35+2*repeated_line_r,all_out_right[Lost_line_r],35+2*Lost_line_r,65);
//                  scan_mid[3]=(right[3]+left[3])/2;
//                  value[3]=COL/2-scan_mid[3];
//              }
//          else if(Lost_line_l>0&&repeated_line_l>0)
//              {
//                  left[3]=linear(all_out_left[repeated_line_l],35+2*repeated_line_l,all_out_right[Lost_line_l],35+2*Lost_line_l,65);
//                  scan_mid[3]=(right[3]+30);
//                  value[3]=COL/2-scan_mid[3];
//              }
//          else if(repeated_line_r>0&&Lost_line_r>0)
//              {
//                  right[3]=linear(all_out_left[repeated_line_r],35+2*repeated_line_r,all_out_right[Lost_line_r],35+2*Lost_line_r,65);
//                  scan_mid[3]=(right[3]-30);
//                  value[3]=COL/2-scan_mid[3];
//              }
       if(flag[2]==1&&flag[0]==1)//ֱ��
      {
        left[3]=linear(left[2],55,left[0],35,65);
        right[3]=linear(right[2],55,right[0],35,65);
        scan_mid[3]=(right[3]+left[3])/2;
        value[3]=COL/2-scan_mid[3];
                value_old_s= all_out_value[13];
      }
      else if(flag[1]==1&&flag[0]==2)//�������б��  //�ұ߱�־��0Ϊ�Ҳ����ߣ�1Ϊ�ҵ������ߣ�2Ϊ�ҵ���ߣ�3Ϊ�ҵ��ұ�
      { //55���� 35���
        left[3]=linear(left[1],55,left[0],35,65);
        right[3]=linear(right[1],55,right[1]+15,35,65);
        scan_mid[3]=(right[3]+left[3])/2;
        value[3]=COL/2-scan_mid[3];
                value_old_s= all_out_value[13];
      }
      else if(flag[1]==1&&flag[0]==3)//�������б��
      {
        left[3]=linear(left[1],55,left[1]-15,35,65);
        right[3]=linear(right[1],55,right[0],35,65);
        scan_mid[3]=(right[3]+left[3])/2;
        value[3]=COL/2-scan_mid[3];
                value_old_s= all_out_value[13];
      }
//      else if(flag[4]==1&&edge_Width[4]<80&&flag[5]==1&&edge_Width[5]<75)//��ʮ��
//      {
//        left[3]=linear(left[5],79,left[4],75,65);
//        right[3]=linear(right[5],79,right[4],75,65);
//        scan_mid[3]=(right[3]+left[3])/2;
//        value[3]=COL/2-scan_mid[3];
//        value_old_s= all_out_value[13];
//      }
//
        else if(flag[1]==1)
        {
            value[3]=value[1]*1.2;
            value_old_s= value[1];
        }
        else
        {
                    value[3]=0;
        }
       if( value[3]>8)
           value[3]=8;
       if( value[3]<-8)
           value[3]=-8;
      }
     }
}


//*******************************************Բ��Ԫ��*************************************************************/
void new_Ring_Line_init()
{
    uint8 i = 0;
    for(i = 0;i < 60;i++)
    {
        Ring_right[i] = -1;
        Ring_left[i] = -1;
    }
}
/*
 *  @�������ƣ�void ����Ѳ��
 * */
void new_Ring_LineFind()
{
    /* ��֪ right��188��ʼ��left��0��ʼ */
    uint8 i = 0;
    uint8 j = 0;
    /* ����� */
    bool diyicishaoxian = false;
    for(i = 0; i<60 ;i++)
    {

        /* ����ǵ�һ��ɨ�ߣ����õ�һ�е��м��� */
        if(!diyicishaoxian)
        {
            j = 94;
            diyicishaoxian = true;
        }
        else
        {
            j = COL/2 - (Ring_left[i]+Ring_right[i]);
        }

        /* �ò�Ⱥͽ�����ɨ�ߣ���94�����ɨ  */
        for(;j>=4;j--)
        {
            if(left_cbh(j,i)>CBH_YUZHI)
            {
                Ring_left[i] = j;
                break;
            }
        }
        /* �ò�Ⱥͽ�����ɨ�ߣ�����һ�е�������ұ�ɨ  */
        for(j = Ring_left[i];j<COL-4;j++)
        {
            if(left_cbh(j,i)>CBH_YUZHI)
            {
                Ring_right[i] = j;
                break;
            }
        }

    }
}
/* @�������ƣ�void new_Ring_offset(void)
 * @����˵����19�쾵ͷ��Բ��ʶ����
 *  */
uint8 zhizou_jilu = 0;//ֱ��ʱ���߲������¼
uint8 zhizou_zuobian = 0;//ֱ��ʱ���15�к����ҵ��߼�¼
void new_Ring_offset(void)
{
    int a,b,i,j;
    int left_rk=0,right_rt=0;
    int left_rj=0,right_rj=0,left_rf=0,right_rf=0,left_rx=0,right_rx=0,left_bmx_bzw=0,right_bmx_bzw=0;

    static uint8 count_ring=0;
    /* Ring_Flag==0:���Բ����־δ����
     * Cross_Flag==0:ʮ�ֱ�־δ����
     * Width_count_num>2000:ɨ���ĵ�ǰͼ�İ׵�ĸ�������2000
     * break_flag>19:���һ�ζ�������19�к�
     * Lost_line_l>0:��߶���>0
     * Lost_line_r==0||Lost_line_r==23:�ұ߲���������ұ߶�����Ϊ23
     * repeated_line_r:�ұ�û�и���
     * all_out_right[10]-all_out_right[18]<30:������ֱ�߱�
     * right[6]:��ȡǰհ���ұߵ�����
     * right[3]==-1||right[3]>160)&&left[3]>0:һ�߶���һ��û�ж���
     * flag:�ұ߱�־
     * edge_Width:ǰհ�еĿ��
     *
     * */
    /* ֻ���������������������� */
    if(     /* ���Բ����־δ������ʮ�ֱ�־δ�������׵�>2000,���һ�ζ�������19�к�*/
            Ring_Flag==0&&Cross_Flag==0&&Width_count_num>2000&&break_flag>19
            /* �����Ϊ�����������ж��㣬�����ұ�û��������ұ߶�����23�У������ұ�û�и��㣨��ʾ�ұ��������ĵ㣩�������ұߵ�10�е�����-�ұߵ�18�е�����<30(˵����ֱ��)*/
         &&((Lost_line_l>0&&(Lost_line_r==0||Lost_line_r==23)&&repeated_line_r==0&&all_out_right[10]-all_out_right[18]<30)
                 ||(Lost_line_r>0&&(Lost_line_l==0||Lost_line_l==23)&&repeated_line_l==0&&all_out_left[18]-all_out_left[10]<30))
//
         &&((    /* �ұ߶������û�� ���� ��߶����ұ�û�� */
                 (right[3]==-1||right[3]>160)&&left[3]>0)||((left[3]==-1||left[3]<20)&&right[3]>0)
                 /* 16�е��ұ߶������û�� ���� 16�е���߶����ұ�û���� */
                 ||((all_out_right[16]==-1||all_out_right[16]>160)&&all_out_left[16]>0)||((all_out_left[16]==-1||all_out_left[16]<20)&&all_out_right[16]>0)
                 /* 17�е��ұ߶������û�� ���� 17�е���߶����ұ�û���� */
                 ||((all_out_right[17]==-1||all_out_right[17]>160)&&all_out_left[17]>0)||((all_out_left[17]==-1||all_out_left[17]<20)&&all_out_right[17]>0)
                 /* 0,1,2,4,5�ж��߳��֣�����3û�ж��߲����м��ɫ�п��>105 */
                 ||(!(flag[0]==1&&flag[1]==1&&flag[2]==1&&flag[4]==1&&flag[5]==1)&&(flag[3]==1&&edge_Width[3]>105))
 //    ||(break2_flag<6&&flag[3]==1&&all_out_flag[16]==1)
                 /* �м��кͺ�������ж����ҵ��� ����15�е��������� */
                 ||(flag[3]==1&&all_out_flag[16]==1&&all_out_flag[14]==1&&all_out_right[15]<all_out_right[16]&&all_out_right[15]<all_out_right[14])
                 /* �м��кͺ�������ж����ҵ��� ����15�е��������� */
                 ||(flag[3]==1&&all_out_flag[16]==1&&all_out_flag[14]==1&&all_out_left[15]>all_out_left[16]&&all_out_left[15]>all_out_left[14]))//��һ�߷������㵫����һ��û��

    )
    {
        /* ��5�е�23�п�ʼɨ�� */
        for(j=5;j<23;j++)
        {
            /* �ӵ����е���23�У��ı߲����߼��Ǹ�*/
            if(all_out_left[j]!=-1)
                left_rj++;
            if(all_out_right[j]!=-1)
                right_rj++;
//        if(j>13)
//          {
            /* ��¼�ҵ���ߵ��� */
            if(all_out_flag[j]==2)
                left_rf++;
            /* ��¼�ҵ��ұߵ��� */
            if(all_out_flag[j]==3)
                right_rf++;
//          }
            /* ��߲�������� */
            if(all_out_left[j]<all_out_left[j+1])
                left_rx++;
            /* �ұ߲�������� */
            if(all_out_right[j]>all_out_right[j+1])
                right_rx++;
            /* */
            if(
                    (
                            (all_out_left[j-2]+5<all_out_left[j]&&all_out_left[j]>all_out_left[j+2]+5)
                            ||(all_out_left[j-2]<all_out_left[j+2]&&all_out_left[j]<0)
                    )
                    &&(all_out_flag[j+2]==1&&all_out_flag[j-2]==1)
              )
                left_bmx_bzw++;
            if(
                    (
                            (all_out_right[j-2]-5>all_out_right[j]&&all_out_right[j]<all_out_right[j+2]-5)
                            ||(all_out_right[j-2]>all_out_right[j+2]&&all_out_right[j]<0)
                            )
                            &&(all_out_flag[j+2]==1&&all_out_flag[j-2]==1)
                     )
                right_bmx_bzw++;
        }
        /* ���ж� */
        if(
                (Lost_line_r>0||(break2_flag<6&&flag[3]==1))
                &&Lost_line_l>18&&repeated_line_l==0
                &&left_rx>13&&left_rj>8
                &&(left_rf>8)&&right_rx>4
                &&right_rx<9&&(left_bmx_bzw<1)&&break2_flag<20
          )
        {
            left_rk=(int)(slope_k1(all_out_left[break2_flag+1],35+2*break2_flag+2,all_out_left[5],45)*100);
            right_rt=(int)(slope_k1(all_out_right[break2_flag+1],35+2*break2_flag+2,all_out_right[5],45)*100);
            if(
                    (left_rk<160&&left_rk>70&&right_rt<-500&&right_rt>-4100)
                    ||(left_rk<270&&left_rk>70&&right_rt==0&&break2_flag<6
                    &&(all_out_right[16]-all_out_right[19]<35&&all_out_right[16]-all_out_right[19]>17&&all_out_right[17]>all_out_right[19]&&all_out_right[16]>all_out_right[17]&&all_out_flag[19]==1&&all_out_flag[16]==1&&all_out_flag[17]==1)
                    )
            )
            {                                                                                                                 //�ж���б�ʺ���б��
                Ring_Flag=2;//�󻷱�־
                first_in=1;
                Ringint=0;
                Bee_on();

            }
        }
        if(Lost_line_l>0&&Lost_line_r==0&&repeated_line_r==0
                &&right_rx>13&&right_rj>8&&(right_rf>8)&&left_rx>4&&(right_bmx_bzw<1)&&break2_flag<20)
        {
            left_rk=(int)(slope_k1(all_out_left[break2_flag+1],35+2*break2_flag+2,all_out_left[5],45)*100);
            right_rt=(int)(slope_k1(all_out_right[break2_flag+1],35+2*break2_flag+2,all_out_right[5],45)*100);
            if(left_rk<-30&&left_rk>-320&&right_rt<-70&&right_rt>-160)
            {
                Ring_Flag=3;//�һ���־
                first_in=1;
                Ringint=0;
                Bee_on();
            }
        }
    }
    /******************************ֱ������*************************************/
   if(Ring_Flag==2)//Բ����������ұ���
   {
       scan_mid[3]=left[3]+31;//44+45=94,����49��//31
       value[3]=(float)(COL/2-scan_mid[3]);
       /* ��0��15��¼�ҵ����ߵ��� */
       zhizou_jilu = 0;
       for(j=0;j<15;j++)
       {
           if(all_out_flag[j]==1)
               zhizou_jilu ++;
       }

   }
   if(Ring_Flag==3)//Բ���ұ����������
   {
      scan_mid[3]=right[3]-37;//138-44=94,����138��
      value[3]=(float)(COL/2-scan_mid[3]);
      /* ��0��15��¼�ҵ����ߵ��� */
      for(j=0;j<15;j++)
      {
          if(all_out_flag[j]==1)
              zhizou_jilu ++;
      }

   }
   /*********************************ֱ������***********************************/
   /*********************************ʶ���뻷��***********************************/
   if(Ring_Flag==2 && zhizou_jilu>12
      &&edge_Width[3]>100
      &&edge_Width[4]>100
      &&edge_Width[5]>100
      )
   {
       Ring_Flag=4;
   }
   /*********************************ʶ���뻷��***********************************/
   /*********************************�뻷����***********************************/
   if(Ring_Flag==4)
   {
       scan_mid[3]=left[3]+94;//44+45=94,����49��//31
       value[3]=(float)(COL/2-scan_mid[3]);
       Speed_Aim = 60;

   }
   //��С�д��������У���ʵ������������ʶ���ˣ�����������֤���Ѿ�������������������԰�15��14��Сһ��
   if(Ring_Flag==4&&((all_out_left[15]>all_out_left[14]&&Width_count_num<1750)||(Ring_Flag==4&&Ring_Roll<-45)))
   {
       Ring_Flag=6;
       Bee_off();
       Bee_on();
       Ring_plush=0;
       Ring_in_Flag=0;
   }
   if(Ring_Flag==6)
   {


       value[3]=value[0];
       Bee_turn();
   }
   /*********************************�뻷����***********************************/
}
void Ring_offset(void)
{
   int a,b,i,j;
   int left_rk=0,right_rt=0;
   int left_rj=0,right_rj=0,left_rf=0,right_rf=0,left_rx=0,right_rx=0,left_bmx_bzw=0,right_bmx_bzw=0;
   static uint8 count_ring=0;
//  oled_printf_int32(64,0,(int)(slope_k(all_out_left[21],77,all_out_left[10],55)*100),5);
//  oled_printf_int32(64,2,(int)(slope_k(all_out_right[21],77,all_out_right[10],55)*100),5);

//              oled_show_int(73,0,(int)(slope_k1(all_out_left[21],77,all_out_left[5],45)*100),4);
//              oled_show_int(73,2,(int)(slope_k1(all_out_right[21],77,all_out_right[5],45)*100),4);
//   if(break2_flag<18){
//           oled_show_int(73,0,(int)(slope_k1(all_out_left[break2_flag+1],35+2*break2_flag+2,all_out_left[5],45)*100),4);
//           oled_show_int(73,2,(int)(slope_k1(all_out_right[break2_flag+1],35+2*break2_flag+2,all_out_right[5],45)*100),4);
//   }
//  oled_printf_int32(32,0,(int)(,5);
//  if(Ring_Flag==0&&Cross_Flag==0&&(((right[3]==-1||right[3]>150)&&left[3]>0)||((left[3]==-1||left[3]<26)&&right[3]>0)
//      ||(!(flag[0]==1&&flag[1]==1&&flag[2]==1&&flag[4]==1&&flag[5]==1)&&(flag[3]==1&&edge_Width[3]>105)))
//  &&abs(all_out_scan_mid[10]-all_out_scan_mid[20])>25&&abs(all_out_scan_mid[12]-all_out_scan_mid[11])<6&&abs(all_out_scan_mid[22]-all_out_scan_mid[21])<6
//  &&(all_out_flag[22]==1||all_out_flag[23]==1)&&((all_out_left[20]<all_out_left[5]&&all_out_right[20]>all_out_right[5])||(all_out_right[20]<all_out_right[5]&&all_out_left[20]>all_out_left[5])))
//  oled_show_int(0, 2, Lost_line_l, 4);
//  oled_show_int(0, 4, Lost_line_r, 4);
//  oled_show_int(0, 6, repeated_line_r, 4);
//
//  oled_show_int(42, 2, repeated_line_l, 4);
//   oled_show_int(41,4,(int)(Lost_line_r),3);
//          oled_show_int(41,6,(int)(Lost_line_l),3);
//
//          oled_show_int(73,4,(int)(repeated_line_r),2);
//          oled_show_int(73,6,(int)(repeated_line_l),2);
   /* Ring_Flag==0:���Բ����־δ����
    * Cross_Flag==0:ʮ�ֱ�־δ����
    * Width_count_num>2000:ɨ���ĵ�ǰͼ�İ׵�ĸ�������2000
    * break_flag>19:���һ�ζ�������19�к�
    * Lost_line_l>0:��߶���>0
    * Lost_line_r==0||Lost_line_r==23:�ұ߲���������ұ߶�����Ϊ23
    * repeated_line_r:�ұ�û�и���
    * all_out_right[10]-all_out_right[18]<30:������ֱ�߱�
    * right[6]:��ȡǰհ���ұߵ�����
    * right[3]==-1||right[3]>160)&&left[3]>0:һ�߶���һ��û�ж���
    * flag:�ұ߱�־
    * edge_Width:ǰհ�еĿ��
    *
    * */
   if(Ring_Flag==0&&Cross_Flag==0&&Width_count_num>2000&&break_flag>19
        &&((Lost_line_l>0&&(Lost_line_r==0||Lost_line_r==23)&&repeated_line_r==0&&all_out_right[10]-all_out_right[18]<30)
                ||(Lost_line_r>0&&(Lost_line_l==0||Lost_line_l==23)&&repeated_line_l==0&&all_out_left[18]-all_out_left[10]<30))
//      &&(all_out_flag[22]==1||all_out_flag[23]==1)
        &&(((right[3]==-1||right[3]>160)&&left[3]>0)||((left[3]==-1||left[3]<20)&&right[3]>0)
    ||((all_out_right[16]==-1||all_out_right[16]>160)&&all_out_left[16]>0)||((all_out_left[16]==-1||all_out_left[16]<20)&&all_out_right[16]>0)
    ||((all_out_right[17]==-1||all_out_right[17]>160)&&all_out_left[17]>0)||((all_out_left[17]==-1||all_out_left[17]<20)&&all_out_right[17]>0)
    ||(!(flag[0]==1&&flag[1]==1&&flag[2]==1&&flag[4]==1&&flag[5]==1)&&(flag[3]==1&&edge_Width[3]>105))
//    ||(break2_flag<6&&flag[3]==1&&all_out_flag[16]==1)
    ||(flag[3]==1&&all_out_flag[16]==1&&all_out_flag[14]==1&&all_out_right[15]<all_out_right[16]&&all_out_right[15]<all_out_right[14])
    ||(flag[3]==1&&all_out_flag[16]==1&&all_out_flag[14]==1&&all_out_left[15]>all_out_left[16]&&all_out_left[15]>all_out_left[14]))//��һ�߷������㵫����һ��û��
    )
  {

        for(j=5;j<23;j++)
        {
            /* �ӵ����е���23�У��ı߲����߼��Ǹ�*/
            if(all_out_left[j]!=-1)
                left_rj++;
            if(all_out_right[j]!=-1)
                right_rj++;
//        if(j>13)
//          {
            /* ��¼�ҵ���ߵ��� */
            if(all_out_flag[j]==2)
                left_rf++;
            /* ��¼�ҵ��ұߵ��� */
            if(all_out_flag[j]==3)
                right_rf++;
//          }
            if(all_out_left[j]<all_out_left[j+1])
                left_rx++;
            if(all_out_right[j]>all_out_right[j+1])
                right_rx++;

            if(((all_out_left[j-2]+5<all_out_left[j]&&all_out_left[j]>all_out_left[j+2]+5)||(all_out_left[j-2]<all_out_left[j+2]&&all_out_left[j]<0))
                    &&(all_out_flag[j+2]==1&&all_out_flag[j-2]==1))
                left_bmx_bzw++;
            if(((all_out_right[j-2]-5>all_out_right[j]&&all_out_right[j]<all_out_right[j+2]-5)||(all_out_right[j-2]>all_out_right[j+2]&&all_out_right[j]<0))
                    &&(all_out_flag[j+2]==1&&all_out_flag[j-2]==1))
                right_bmx_bzw++;
        }
//        oled_show_int(41,4,(int)(Lost_line_r),3);
//        oled_show_int(41,6,(int)(Lost_line_l),3);
//
//        oled_show_int(73,4,(int)(repeated_line_r),2);
//        oled_show_int(73,6,(int)(repeated_line_l),2);
//

//
//        oled_show_int(0,0,(int)(left_rx),2);
//        oled_show_int(0,2,(int)(left_rj),2);
//        oled_show_int(0,4,(int)(left_rf),2);
//        oled_show_int(0,6,(int)(left_bmx_bzw),2);
//        oled_show_int(25,0,(int)(right_rx),2);
//        oled_show_int(25,2,(int)(right_rj),2);
//        oled_show_int(25,4,(int)(right_rf),2);

//      Bee_on();

//                if((abs(left[2]+left[4]-2*left[3])<5)&&(abs(all_out_left[22]+all_out_left[14]-2*all_out_left[18])<5)&&
////                if((abs(left[2]+left[4]-2*left[3])<5)&&(abs(left[3]+left[5]-2*left[4])<5)&&
//                  left[4]<=left[5]&&left[2]<=left[3]&&left[3]<=left[4]&&left[5]<94&&left[5]>0&&left[4]>0&&left[3]>0&&left[2]>0
//                              &&(right[3]==-1||edge_Width[3]>105)&&(Lost_line_r>0&&Lost_line_l==0&&repeated_line_l==0))//Բ�����  //��5�п�ʼ�ж��ٸ�����ڣ��ж�����ֻ������ڣ��ϸ�����¸��������ж��٣���ʵ�������ж�б��
                if((Lost_line_r>0||(break2_flag<6&&flag[3]==1))&&Lost_line_l>18&&repeated_line_l==0
                    &&left_rx>13&&left_rj>8&&(left_rf>8)&&right_rx>4&&right_rx<9&&(left_bmx_bzw<1)&&break2_flag<20
                    )
                {
//                    oled_show_int(73,0,(int)(slope_k1(all_out_left[break2_flag+1],35+2*break2_flag+2,all_out_left[5],45)*100),4);
//                    oled_show_int(73,2,(int)(slope_k1(all_out_right[break2_flag+1],35+2*break2_flag+2,all_out_right[5],45)*100),4);

//                                    left_rk=(int)(slope_k1(all_out_left[21],77,all_out_left[10],45)*100);
//                                    right_rt=(int)(slope_k1(all_out_right[21],77,all_out_right[10],45)*100);//�ϰ汾

                            left_rk=(int)(slope_k1(all_out_left[break2_flag+1],35+2*break2_flag+2,all_out_left[5],45)*100);
                            right_rt=(int)(slope_k1(all_out_right[break2_flag+1],35+2*break2_flag+2,all_out_right[5],45)*100);


//                  a=linear(all_out_scan_mid[1],45,all_out_scan_mid[0],35,83);
////                  b=linear(all_out_left[20],75,all_out_left[15],65,81);
//                  b=linear(all_out_left[15],65,all_out_left[10],55,81);
//                  if(((left_rk<110&&left_rk>0&&right_rt==0)||
//                                        (left_rk<110&&right_rt<-400)))//�ж�83���ǲ��ǰ�ɫ//�ϰ汾
                        if((left_rk<160&&left_rk>70&&right_rt<-500&&right_rt>-4100)
                                ||(left_rk<270&&left_rk>70&&right_rt==0&&break2_flag<6
                                        &&(all_out_right[16]-all_out_right[19]<35&&all_out_right[16]-all_out_right[19]>17&&all_out_right[17]>all_out_right[19]&&all_out_right[16]>all_out_right[17]&&all_out_flag[19]==1&&all_out_flag[16]==1&&all_out_flag[17]==1)
                                        ))
                  {                                                                                                                 //�ж���б�ʺ���б��
                                        Ring_Flag=2;
                                        first_in=1;
                                        Ringint=0;
                                        Bee_on();

                                    }
                }
//                if((abs(right[2]+right[4]-2*right[3])<5)&&(abs(all_out_right[22]+all_out_right[14]-2*all_out_right[18])<5)&&
////                if((abs(right[2]+right[4]-2*right[3])<5)&&(abs(right[3]+right[5]-2*right[4])<5)&&
//                  right[4]>=right[5]&&right[2]>=right[3]&&right[3]>=right[4]&&right[5]>94&&right[5]>0&&right[4]>0&&right[3]>0
//                              &&right[2]>0&&(left[3]==-1||edge_Width[3]>105)&&Lost_line_l>0&&Lost_line_r==0&&repeated_line_r==0)//Բ���ұ�  (absi(right[2]+right[4]-2*right[3])<8)&&79      55      67
                                if(Lost_line_l>0&&Lost_line_r==0&&repeated_line_r==0
                                    &&right_rx>13&&right_rj>8&&(right_rf>8)&&left_rx>4&&(right_bmx_bzw<1)&&break2_flag<20)
//                                if((Lost_line_r>0||(break2_flag<6&&flag[3]==1))&&Lost_line_l>18&&repeated_line_l==0
//                                &&left_rx>13&&left_rj>8&&(left_rf>4||break2_flag<6)&&right_rx>4&&(left_bmx_bzw<1||break2_flag<6)&&break2_flag<20)

                                {
//                                  Bee_on();
//                                    left_rk=(int)(slope_k1(all_out_left[21],77,all_out_left[10],45)*100);
//                                    right_rt=(int)(slope_k1(all_out_right[21],77,all_out_right[10],45)*100);
                                    left_rk=(int)(slope_k1(all_out_left[break2_flag+1],35+2*break2_flag+2,all_out_left[5],45)*100);
                                    right_rt=(int)(slope_k1(all_out_right[break2_flag+1],35+2*break2_flag+2,all_out_right[5],45)*100);
//                                    oled_show_int(73,0,(int)(slope_k1(all_out_left[break2_flag+1],35+2*break2_flag+2,all_out_left[5],45)*100),4);
//                                oled_show_int(73,2,(int)(slope_k1(all_out_right[break2_flag+1],35+2*break2_flag+2,all_out_right[5],45)*100),4);
//                   a=linear(all_out_scan_mid[1],45,all_out_scan_mid[0],35,83);//�ж�83���ǲ��ǰ�ɫ
//                   b=linear(all_out_right[20],75,all_out_right[15],65,81);
//                   b=linear(all_out_right[15],65,all_out_right[10],55,81);
//                                if(((right_rt>-110&&right_rt<0&&left_rk==0)||
//                                    (right_rt>-110&&left_rk>0&&left_rk<280)))//||(all_out_flag[5]==3&&all_out_flag[21]==3)
                                  if(left_rk<-30&&left_rk>-320&&right_rt<-70&&right_rt>-160)
                  {
                                        Ring_Flag=3;
                                        first_in=1;
                                        Ringint=0;
                                        Bee_on();
                                    }
                }
  }
     /******************************ֱ������*************************************/
//   oled_show_int(25,4,(int)(ad_value[2]),4);
//           oled_show_int(25,6,(int)(Ring_In_adave),4);
//           oled_show_int(25,2,(int)(Ring_Flag),4);
    if(Ring_Flag==2)//Բ����������ұ���
    {

//              count_ring++;
        scan_mid[3]=left[3]+31;//44+45=94,����49��//31
        value[3]=COL/2-scan_mid[3];
//        if(count_ring>180)
//        {
//                  count_ring=0;
//                  Bee_off();
//                  Ring_Flag=0;
//                  Ring_plush=0;
//                  Ring_En_plush=0;
//                  dm_offset_flag=1;
//                  Ring_En=0;
//                  Cross_En=0;
//                  Ramp_En=0;
//                  Destination_En=0;
//                  ForkRoad_En=0;
//        }
    }
    if(Ring_Flag==3)//Բ���ұ����������
    {
//           count_ring++;
       scan_mid[3]=right[3]-37;//138-44=94,����138��
       value[3]=COL/2-scan_mid[3];
//       if(count_ring>180)
//        {
//                  count_ring=0;
//                  Bee_off();
//                  Ring_Flag=0;
//                  Ring_plush=0;
//                  Ring_En_plush=0;
//                  dm_offset_flag=1;
//                  Ring_En=0;
//                  Cross_En=0;
//                  Ramp_En=0;
//                  Destination_En=0;
//                  ForkRoad_En=0;
//        }
    }
    /*********************************ֱ������***********************************/
    /**********************************����м����ֵ����**********************************///||(break2_flag<5&&all_out_flag[18]==1)
    if(Ring_Flag==2&&((ad_value[2]>Ring_In_adave*1.65&&Ring_finish>5000)||(ad_value[0]>1000&&ad_value[1]>1000&&Ring_finish>5000)))//||(all_out_edge_Width[19]>all_out_edge_Width[15]&&all_out_edge_Width[15]<all_out_edge_Width[11])))//�˴���Ϊ����Ϊ���ֵʱ
    {
        Ring_Flag=4;
    }
    if(Ring_Flag==3&&((ad_value[2]>Ring_In_adave*2.1&&Ring_finish>5000)))//||(all_out_edge_Width[19]>all_out_edge_Width[15]&&all_out_edge_Width[15]<all_out_edge_Width[11])))
    {         // &&SystemAttitude.Pitch>16&&SystemAttitude.Pitch<20
        Ring_Flag=5;
    }
   /**********************************�������**********************************/
    /**********************************��������**********************************/
    if(Ring_Flag==4)
    {
         if(all_out_right[15]<175&&all_out_right[15]!=-1&&break2_flag>9)//�ұ߻����õ�
         {
//           value[3]=0-ring_value-all_out_right[15]+155;
             value[3]=all_out_value[15];
             if(value[3]>-ring_value+30)
                 value[3]=-ring_value+30;
          }
        else//������Ѿ�Ҫ�������ˣ����ܸ��ұ���
        {
                    value[3]=0-ring_value;//�̶����
                    lost_Flag=1;
        }
//         if(value[3]>-ring_value+15)
//             value[3]=-ring_value+15;
    }
    if(Ring_Flag==5)
   {
         if(all_out_left[15]>17&&all_out_left[15]!=-1&&break2_flag>9)
//        {
//          value[3]=ring_value-all_out_left[15]+22;
//        }
         {
                 if(value[3]<ring_value-30)
                     value[3]=ring_value-30;
         }
      else
      {
                value[3]=ring_value-4;
                lost_Flag=1;
      }
    }
    //��С�д��������У���ʵ������������ʶ���ˣ�����������֤���Ѿ�������������������԰�15��14��Сһ��
   if(Ring_Flag==4&&((all_out_left[15]>all_out_left[14]&&Width_count_num<1750)||(Ring_Flag==4&&Ring_Roll<-45)))
   {
       Ring_Flag=6;
       Bee_off();
       Ring_plush=0;
       Ring_in_Flag=0;
   }
   if(Ring_Flag==5&&((all_out_right[15]<all_out_right[14]&&Width_count_num<1750)||(Ring_Flag==5&&Ring_Roll>45)))
           {
             Ring_Flag=7;
              Bee_off();
              Ring_plush=0;
              Ring_in_Flag=0;
           }

   /**********************************��������**********************************/
   /**********************************��������**********************************/
  if(Ring_Flag==6)
     {
      value[3]=value[2];
      /********************************Բ��ƫ����*******************************////////////////////
//      scan_mid[3]=right[3]-30;
//       value[3]=COL/2-scan_mid[3];
       if(value[3]>-15)value[3]=-15;
       if(value[3]<-45)value[3]=-45;//�ٶȿ�����ø�
       if(Ringint==0)
        {
          RingAngle=value[3];
          Ringint=1;
        }
        else
            RingAngle=0.9*RingAngle+0.1*(float)value[3];

     }
  if(Ring_Flag==7)
    {
      value[3]=value[2];
      if(value[3]>50)value[3]=50;//�ٶȿ�����ø�
      if(value[3]<15)value[3]=15;
      if(Ringint==0)
        {
          RingAngle=value[3];
          Ringint=1;
        }
        else
       RingAngle=0.9*RingAngle+0.1*(float)value[3];

    }
  if((Ring_Flag==6||Ring_Flag==7)&&Ring_plush>25000)//�������ۼ�
 {
     Ring_plush_flag=1;
 }

  //   oled_show_int(25,4,(int)(ad_value[2]),4);
  //           oled_show_int(25,6,(int)(Ring_In_adave),4);
  //           oled_show_int(25,2,(int)(Ring_Flag),4);
//  if(Ring_Flag==6&&Width_count_num>2000&&Ring_plush_flag==1&&((out[75*COL+all_out_left[14]]>Threshold
//          &&out[75*COL+all_out_left[14]+10]>Threshold+10&&((all_out_left[16]<=all_out_left[15])
//                  ||all_out_left[15]==-1||all_out_left[16]==-1))||Ring_Roll<-285))

//     oled_show_int(0,4,(int)(out[75*COL+all_out_left[14]]-Threshold),4);
//              oled_show_int(0,6,(int)(out[75*COL+all_out_left[14]+10]-Threshold-10),4);
//              oled_show_int(0,2,(int)(all_out_left[16]-all_out_left[15]),4);
//              oled_show_int(43,0,(int)(Ring_Flag),4);
//              oled_show_int(43,2,(int)(Ring_plush_flag),4);
//              oled_show_int(43,4,(int)(Width_count_num),4);
//              oled_show_int(43,6,(int)(Ring_plush),7);
 if((Ring_Flag==6&&Ring_plush_flag==1&&((out[70*COL+all_out_left[14]+5]>Threshold&&out[70*COL+all_out_left[14]]>Threshold-10)||(all_out_value[19]>all_out_value[17]&&all_out_flag[19]!=4))&&(all_out_left[16]<all_out_left[15]))
         ||(Ring_Flag==6&&Ring_Roll<-285)||(Ring_Flag==6&&all_out_value[18]-5>all_out_value[15]&&all_out_flag[18]==1&&Ring_plush_flag==1)
         ||(all_out_flag[16]==4&&all_out_flag[10]==1&&break_flag<16&&Ring_Flag==6))
  {
     Ring_Flag=8;
      Ring_plush_flag=0;
       Bee_on();

 }

  if((Ring_Flag==7&&Ring_plush_flag==1&&out[70*COL+all_out_right[15]]>Threshold-20&&all_out_right[16]>all_out_right[15])||(Ring_Flag==7&&Ring_Roll>285))//295
 {
     Ring_Flag=9;
   Ring_plush_flag=0;
   Bee_on();

  }

  if(Ring_Flag==8)
  {
      //    if(flag[1]!=4&&flag[0]!=4)
      //        value[3]=linear(value[2],45,value[2],35,65);
    if(RingAngle>-10)
      RingAngle=-10;
    if(RingAngle<-40)
      RingAngle=-40;
    value[3]=(int)RingAngle;            //����ƫ����
    for(i=45;i<80;i+=2)
      {
//          if(out[i*COL+all_out_scan_mid[1]]+out[(i+1)*COL+all_out_scan_mid[1]]+out[(i+2)*COL+all_out_scan_mid[1]]-out[(i+3)*COL+all_out_scan_mid[1]]-out[(i+4)*COL+all_out_scan_mid[1]]-out[(i+5)*COL+all_out_scan_mid[1]]>30&&out[(i+3)*COL+all_out_scan_mid[1]]+out[(i+5)*COL+all_out_scan_mid[1]]<2*black+10)
//          break;
                if(((int)((float)(out[i*COL+all_out_scan_mid[0]]+out[(i-1)*COL+all_out_scan_mid[0]]+
                out[(i-2)*COL+all_out_scan_mid[0]]-out[(i+1)*COL+all_out_scan_mid[0]]-
                out[(i+2)*COL+all_out_scan_mid[0]]-out[(i+3)*COL+all_out_scan_mid[0]])/
    (float)(out[i*COL+all_out_scan_mid[0]]+out[(i-1)*COL+all_out_scan_mid[0]]+
                out[(i-2)*COL+all_out_scan_mid[0]]+out[(i+1)*COL+all_out_scan_mid[0]]+
                out[(i+2)*COL+all_out_scan_mid[0]]+out[(i+3)*COL+all_out_scan_mid[0]])*100)>6)
                         &&out[(i+1)*COL+all_out_scan_mid[0]]+out[(i+3)*COL+all_out_scan_mid[0]]<2*black+20
                         &&out[(i)*COL+all_out_scan_mid[0]]+out[(i-2)*COL+all_out_scan_mid[0]]<2*white)
                break;
      }
    if(i<77)
      value[3]=-ring_value;//���������������
//     if(left[3]!=-1&&all_out_left[16]>left[3]&&left[3]+45>94-value[3]){
//      value[3]=94-(left[3]+45);
//        }
//    if()
        if(flag[1]!=4&&flag[0]!=4)
      value[3]=linear(value[1],45,value[0],35,65)+5;
    if(all_out_left[15]>all_out_left[14]&&all_out_left[14]>all_out_left[13]){
         value[3]=all_out_value[15]+5;
           }
//    else if((right[3]<178&&right[3]!=-1)||(right[1]<172&&right[1]!=-1))
//      {for(i=20;i>=10;i--)
//      {if(!(all_out_left[i]>=all_out_left[i-1]&&all_out_left[i-1]!=-1&&all_out_left[i]-all_out_left[i-1]<=6))
//         break;}
//      if(i<=15)
//        value[3]=COL/2-(left[3]+right[3])/2+8;//+5
//                Bee_on();
//      }
     if(value[3]<-45)
         value[3]=-45;//���Ƴ����ķ�
     if(value[3]>-15)
         value[3]=-15;
  }
  if(Ring_Flag==9)
  {
    if(RingAngle<10)
      RingAngle=10;
    if(RingAngle>40)
      RingAngle=40;
            value[3]=(int)RingAngle-2;            //����ƫ����
    for(i=45;i<80;i+=2)
      {
//          if(out[i*COL+all_out_scan_mid[1]]+out[(i+1)*COL+all_out_scan_mid[1]]+out[(i+2)*COL+all_out_scan_mid[1]]-out[(i+3)*COL+all_out_scan_mid[1]]-out[(i+4)*COL+all_out_scan_mid[1]]-out[(i+5)*COL+all_out_scan_mid[1]]>30&&out[(i+3)*COL+all_out_scan_mid[1]]+out[(i+5)*COL+all_out_scan_mid[1]]<2*black+10)
//          break;
                if(((int)((float)(out[i*COL+all_out_scan_mid[0]]+out[(i-1)*COL+all_out_scan_mid[0]]+
                out[(i-2)*COL+all_out_scan_mid[0]]-out[(i+1)*COL+all_out_scan_mid[0]]-
                out[(i+2)*COL+all_out_scan_mid[0]]-out[(i+3)*COL+all_out_scan_mid[0]])/
    (float)(out[i*COL+all_out_scan_mid[0]]+out[(i-1)*COL+all_out_scan_mid[0]]+
                out[(i-2)*COL+all_out_scan_mid[0]]+out[(i+1)*COL+all_out_scan_mid[0]]+
                out[(i+2)*COL+all_out_scan_mid[0]]+out[(i+3)*COL+all_out_scan_mid[0]])*100)>6)
                         &&out[(i+1)*COL+all_out_scan_mid[0]]+out[(i+3)*COL+all_out_scan_mid[0]]<2*black+20
                         &&out[(i)*COL+all_out_scan_mid[0]]+out[(i-2)*COL+all_out_scan_mid[0]]<2*white)
                break;
      }
    if(i<77)
      value[3]=ring_value;
    if(all_out_right[16]!=-1&&all_out_right[16]<right[3]&&right[3]-45<94-value[3]){//���õ��ұ߿����ұ�����
      value[3]=94-(right[3]-50);
     }
    else if(left[3]>10||left[1]>15)
       { for(i=20;i>=10;i--)
          {if(!(all_out_right[i]<=all_out_right[i-1]&&all_out_right[i]!=-1&&all_out_right[i-1]-all_out_right[i]<=6))
            break;
          }
          if(i<=15)
       value[3]=COL/2-(left[3]+right[3])/2-8;
//                                Bee_on();
       }
    if(value[3]>40)
     value[3]=40;//���Ƴ����ķ�
    if(value[3]<0)
     value[3]=0;
}

  /**********************************��������**********************************/
  /**********************************��������**********************************/
//    oled_show_int(0,2,(int)(ad_value[2]),4);
//    oled_show_int(0,4,(int)(Ring_In_adave),4);
//    oled_show_int(0,6,(int)(Ring_Roll),4);
//    oled_show_int(43,0,(int)(Ring_Flag),4);
//    oled_show_int(43,2,(int)(Ring_plush_flag),4);
//    oled_show_int(43,4,(int)(Width_count_num),4);
//    oled_show_int(43,6,(int)(Ring_plush),7);
  if(Ring_Flag==8&&(((ad_value[2]>Ring_In_adave*1.4&&Ring_Roll<-320)||(Ring_Flag==8&&Ring_Roll<-350))))//(abs(all_out_left[12]+all_out_left[22]-2*all_out_left[17])<7)&&all_out_left[17]<all_out_left[22]&&all_out_left[12]<all_out_left[17]&&all_out_left[12]>0&&all_out_left[17]<72)||
  {
        Ring_Flag=10;
        Ring_plush=0;
        lost_Flag=0;
        Bee_on();
  }
   if(Ring_Flag==9&&((((ad_value[2]>Ring_In_adave*1.4&&Ring_Roll>320)||(Ring_Flag==8&&Ring_Roll>350)))))//abs(all_out_right[12]+all_out_right[22]-2*all_out_right[17])<7)&&all_out_right[12]>all_out_right[17]&&all_out_right[17]>all_out_right[22]&&all_out_right[22]>0&&all_out_right[17]>115)||
  {
        Ring_Flag=11;
        Ring_plush=0;
        lost_Flag=0;
  }
   if(Ring_Flag==10)//Բ����������ұ���
    {
        scan_mid[3]=left[3]+32;//44+45=94,����49��
        value[3]=COL/2-scan_mid[3];
//            if(Ring_Roll<-355){
//                  if(value[3]<-10)
//                  value[3]=-10;
//              }
        if(value[3]<-40)
        value[3]=-40;//���Ƴ����ķ�
    }
    if(Ring_Flag==11)//Բ���ұ����������
    {
        scan_mid[3]=right[3]-36;//138-44=94,����138��
         value[3]=COL/2-scan_mid[3];
//              if(Ring_Roll>355){
//                  if(value[3]>10)
//                  value[3]=10;
//              }
        if(value[3]>40)
         value[3]=40;//���Ƴ����ķ�
    }

  if(((Ring_Flag==10&&flag[3]==1&&all_out_flag[13]==1&&all_out_edge_Width[15]>all_out_edge_Width[18]&&all_out_flag[18]==1&&zhi_plush>10000)||(Ring_Roll<-355&&Ring_plush>Ring_finish-20000))
        ||((flag[3]==1&&all_out_flag[13]==1&&all_out_flag[18]==1&&all_out_edge_Width[15]>all_out_edge_Width[18]&&Ring_Flag==11&&zhi_plush>10000)||((Ring_Roll>355&&Ring_plush>Ring_finish-20000))))
     {
      Bee_off();
      Ring_Flag=0;
      Ring_plush=0;
      Ring_finish=0;
      Ring_En_plush=0;
      dm_offset_flag=1;
      Ring_En=0;
      zhi_plush=0;
      Ramp_En=0;
      Ring_Roll=0;
      yuansujis();//����·
      ForkRoad_En=1;
     }
}//����������
/****************************************����������***********************************************************/
int linear(int x1,int y1,int x2,int y2,int y)
{
    int x=0;
    x=((y-y1)*(x1-x2)/(y1-y2))+x1;

    return x;
}
/*******************************************ֱ��(����)*************************************************************/
void AddSpeedCrazy(void)
{
    int add_left_75=0;
    int add_right_75=0;
        add_left_75=linear(all_out_left[10],55,all_out_left[15],65,75) - all_out_left[20];
        add_right_75=all_out_right[20] - linear(all_out_right[10],55,all_out_right[15],65, 75);
        if((((abs(all_out_value[22])>15||(abs(all_out_value[21])>13))&&((add_left_75>10)||(add_right_75>10)))||(Line_down())))//&&((!(abs(all_out_left[22]+all_out_left[14]-2*all_out_left[18])<5))||(!(abs(all_out_right[22]+all_out_right[14]-2*all_out_right[18])<5))))
        {
            AddSpeedCrazy_flag=1;  //����
//          Bee_on();
        }
        else if(((all_out_flag[21]==1)||(all_out_flag[22]==1))&&all_out_flag[17]==1&&all_out_flag[18]==1)
        {
//          Bee_off();
            AddSpeedCrazy_flag=2;  //����
        }
        else
        {
//          Bee_off();
            AddSpeedCrazy_flag=0;  //ԭ��
        }
}
/*******************************************ֱ��(����)*************************************************************/
void Line_acceleration(void)
{
  static int Line_long_flag=0;
  static char Line_flag_old=0;
  Line_flag_old=Line_flag;
  if(ForkRoad_Flag==0)
  {
    if(flag[4]==1&&flag[3]==1&&flag[2]==1&&flag[1]==1&&value[3]<=12&&
       all_out_left[23]<=88&&all_out_right[23]>=100&&
      (abs((all_out_left[22]+all_out_left[6])/2-all_out_left[14])<=3||
       abs((all_out_right[22]+all_out_right[6])/2-all_out_right[14])<=3) )
    {
      Line_add_flag=1;    //�����ۼӱ�־
      //***************************��ֱ������**********************************
      if(line_distance_cnt>=23) //�������ۼ���ֵ 10000  �����ٶ� 50 flag=3 55 3->1 60
      {
        Line_flag=1;        //ֱ����ʼʱ
        Line_long_flag=1;
//        Bee_on();
      }
      //*********************ʶ��ֱ���������ǳ�ֱ��*****************************
      else
      {
        Line_flag=3;
      }
    }
    //**************************��ֱ����һ�����********************************
    else if((Line_long_flag==1&&((all_out_left[23]<=30&&all_out_right[23]<=105)||(all_out_left[23]>=83&&all_out_right[23]>=158)||Line_down()))||Line_long_flag==2)
    {
                Line_flag=2;       //ֱ������ʱ
        Line_long_flag=2;
        line_distance_cnt=0;
        Line_add_flag=0;
//        Bee_off();
      if(Speed_down>30000&&((flag[3]==1&&flag[1]==1&&flag[2]==1&&flag[4]==1)||(flag[3]==1&&flag[1]==1&&flag[2]==1&&flag[4]==1&&value[3]<=12)))
          {
            Line_long_flag=0;
            Speed_down=0;
            //Bee_off();
          }

    }
    else                        //������������϶������ϣ�����ԭ�ٶ���
    {
      Line_flag=3;   //ԭ����
      Speed_Line=0;
      Line_add_flag=0;
      //Bee_off();
    }
  }
  else
  {
    Line_long_flag=0;
    Speed_Line=0;
    Line_add_flag=0;
    Line_flag=3;
  }
}
_Bool Line_down(void)
{
//  if((out[edge_Width[3]*COL+scan_mid[3]+5]<black+10)&&(out[edge_Width[3]*COL+scan_mid[3]]<black+10)&&(out[edge_Width[3]*COL+scan_mid[3]-5]<black+10))
      if((out[edge_Width[3]*COL+scan_mid[3]+5]<Threshold+10)&&(out[edge_Width[3]*COL+scan_mid[3]]<Threshold+10)&&(out[edge_Width[3]*COL+scan_mid[3]-5]<Threshold+10))
    return 1;
  else
    return 0;
}
/******************************************���µ�Ԫ��*********************************************************/
void Big_Ramp(void)
{
    int rampp=0;
  uint16 hang_85,hang_75,i,hang_85_left=-1,hang_85_right=-1;
    if(Ramp_En==1&&Cross_Flag==0&&Ring_Flag==0&&Destination_Flag==0&&ForkRoad_Flag==0)
    {
                if(Ramp_Flag==0)
                {
                    if(flag[3]==1&&edge_Width[3]>75&&edge_Width[4]>60&&flag[4]==1&&edge_Width[5]>55&&flag[5]==1&&edge_Width[3]>edge_Width[4]&&edge_Width[4]>edge_Width[5]&&abs(scan_mid[3]-94)<20)
                    {
                        hang_85=linear(scan_mid[4],75,scan_mid[3],65,85);
                        hang_75=linear(scan_mid[3],65,scan_mid[1],45,75);
                        for(i=hang_85;i>10;i--)
                        {
                            if((out[85*COL+i]+out[85*COL+i+1]+
                             out[85*COL+i+2]-out[85*COL+i-1]-
                             out[85*COL+i-2]-out[85*COL+i-3]>30)&&out[85*COL+i-1]+out[85*COL+i-3]<2*black+30&&out[85*COL+i+2]+out[85*COL+i]<2*white)
                             {
                                hang_85_left=i;
                                break;
                             }
                        }
                         for(i=hang_85;i<177;i++)
                        {
                            if((out[85*COL+i]+out[85*COL+i-1]+
                             out[85*COL+i-2]-out[85*COL+i+1]-
                             out[85*COL+i+2]-out[85*COL+i+3]>30)&&out[85*COL+i+2]+out[85*COL+i]<2*black+30&&out[85*COL+i-1]+out[85*COL+i-3]<2*white)
                             {
                                hang_85_right=i;
                                break;
                             }
                        }
                        if(out[91*COL+hang_85]>Threshold+5&&hang_85_right>0&&hang_85_left>0&&(hang_85_right-hang_85_left)>50&&abs(hang_75-scan_mid[4])<18&&hang_85>60&&hang_85<128)//&&out[75*COL+all_out_scan_mid[20]]>Threshold+10)
                        {
                             Ramp_Flag=2;
                             Bee_on();
                        }
                    }
                }
            if(Ramp_Flag!=0)
            {
                scan_mid[3]=linear(scan_mid[2],55,scan_mid[1],45,65);
                value[3]=(int)(((float)(94-scan_mid[3])));
                if(value[3]>15)
                value[3]=15;
                if(value[3]<-15)
                value[3]=-15;
            }
//            if(Ramp_Flag==2&&((Ramp_time>Big_Ramp_Time)))//ԭ��
            if(Ramp_Flag==2)
            {
                 Ramp_Flag=3;
                 Ramp_time=0;
            }
            if(Ramp_Flag==3)
            {
                Ramp_Flag=0;
                Crazy_Add=0;
                Bee_off();
                Ring_En=0;
                Cross_En=0;
                Ramp_En=0;
                ForkRoad_En=0;
                dm_offset_flag=1;
                Ramp_add_time=0;

            }
    }

}
/******************************************С�µ�Ԫ��*********************************************************/
void Small_Ramp(void)
{
  uint16 hang_91,hang_75,i,hang_91_left=-1,hang_91_right=-1;
    static uint8 count_ramp=0;
    if(Ramp_En==1&&Cross_Flag==0&&Ring_Flag==0&&Destination_Flag==0&&ForkRoad_Flag==0)
    {
            if(Ramp_Flag==0)
            {
                if(flag[3]==1&&edge_Width[3]>80&&edge_Width[4]>70&&flag[4]==1&&edge_Width[5]>60&&flag[5]==1&&edge_Width[3]>edge_Width[4]&&edge_Width[4]>edge_Width[5]&&abs(scan_mid[3]-94)<15)
                {
                    hang_91=linear(scan_mid[5],79,scan_mid[3],65,91);
                    hang_75=linear(scan_mid[3],65,scan_mid[1],45,75);
                    for(i=hang_91;i>10;i--)
                    {
                        if((out[91*COL+i]+out[91*COL+i+1]+
                         out[91*COL+i+2]-out[91*COL+i-1]-
                         out[91*COL+i-2]-out[91*COL+i-3]>30)&&out[91*COL+i-1]+out[91*COL+i-3]<2*black+30&&out[91*COL+i+2]+out[91*COL+i]<2*white)
                         {
                            hang_91_left=i;
                            break;
                         }
                    }
                     for(i=hang_91;i<177;i++)
                    {
                        if((out[91*COL+i]+out[91*COL+i-1]+
                         out[91*COL+i-2]-out[91*COL+i+1]-
                         out[91*COL+i+2]-out[91*COL+i+3]>30)&&out[91*COL+i+2]+out[91*COL+i]<2*black+30&&out[91*COL+i-1]+out[91*COL+i-3]<2*white)
                         {
                            hang_91_right=i;
                            break;
                         }
                    }
                    if(out[91*COL+hang_91]>Threshold+5&&hang_91_right>0&&hang_91_left>0&&(hang_91_right-hang_91_left)>50&&abs(hang_75-scan_mid[4])<18&&hang_91>70&&hang_91<118)//&&out[75*COL+all_out_scan_mid[20]]>Threshold+10)
                    {
                         Ramp_Flag=1;
                         Bee_on();
                    }
                }
            }
            if(Ramp_Flag==1)
            {
                 count_ramp++;
                 if(count_ramp>180)
                 {
                     Ramp_Flag=0;
                     Bee_off();
                     Ring_En=0;
                     Cross_En=0;
                     Ramp_En=0;
                     ForkRoad_En=0;
                     count_ramp=0;
                     dm_offset_flag=1;
                 }
            }
            if(Ramp_Flag==1&&ad_value[2]>Ring_In_adave*1.35)
         {
                    Ramp_Flag=2;
                    Bee_on();
         }
            if(Ramp_Flag!=0)
            {
//              scan_mid[3]=linear(scan_mid[2],55,scan_mid[1],45,65);
                value[3]=(int)(((float)(94-scan_mid[0])*0.5)+(float)XY_Deviation*0.25);
                if(value[3]>10)
                value[3]=10;
                if(value[3]<-10)
                value[3]=-10;
            }//Ramp_time>Big_Ramp_Time*0.5
            if(Ramp_Flag==2&&((1)||(((int)SystemAttitude.Pitch)<-10)))
            {
                 Ramp_Flag=3;
                 Ramp_time=0;
            }
            if(Ramp_Flag==3)
            {
//                EncoderSpeed_Out=0;
//              ChangeSpeed=1;
                Ramp_count++;
                Ramp_En=0;
                Bee_off();
                Ring_En=0;
                Cross_En=0;
                Ramp_Flag=0;
                dm_offset_flag=1;
                Ramp_add_time=15;
            }
    }

}
void R_big(void)          //**************************���������
{
  int i=0;
  Flag=1;
  if(Topoint>7)
  {
    for(i=5;i<Topoint-3;i++)
    {
      if(all_out_left[i]==-1||all_out_right[i]!=-1)
      {
        Flag=0;
        break;
      }
    }
    if(Flag==1&&leftpoint1==0&&rightpoint1==0)
    {
      rightpoint1=2;
    }
  }
}
void L_big(void)         //**************************�����Ҵ���
{
  int i=0;
  Flag=1;
  if(Topoint>7)
  {
    for(i=5;i<Topoint-3;i++)
    {
      if(all_out_left[i]!=-1||all_out_right[i]==-1)
      {
        Flag=0;
        break;
      }
    }
    if(Flag==1&&leftpoint1==0&&rightpoint1==0)
    {
      leftpoint1=2;
    }
  }
}
///*********************************************************FreeCar��غ���***********************************************************/
///*******************************************e*****************************
//*  �������ƣ�sendCamImgToCamViewer
//*  ����˵��������һ��ͼ��freeCar��λ��
//*  ����˵������
//*  �������أ���
//*  �޸�ʱ�䣺2015-1-26
//*  ��    ע��outΪͼ���������
//*************************************************************************/
void sendCamImgToCamViewer(void)
{
  uint8 i,j;
  uint16 offset;
  uint8 start_cmd[4] ={0xFF,0xFE,0xFF,0xFE};//FF,FE,FF,FE�ĸ�����ʾһ��ͼ��Ŀ�ʼ
  uint8 end_cmd[2] = {0xFE,0xFE};

  uart_write_buffer(UART_0, start_cmd, sizeof(start_cmd));    //����һ֡ͼ��Ŀ�ʼ

  for(i = 0;i <120;i++)
  {
    offset = i*188;
    for(j=0;j<188;j++)
    {
      uint8 d = out[offset + j];
      if(d > 0xFD) d = 0xFD;            //�ܿ�У��λ
//      if(d==0)d=0xFD;
//     if(i>34&&i<82&&(((i-35)%10==0)||i==79))
//      {
//        if((i!=79&&j==left[(i-35)/10])||(i==79&&j==left[5]))d=0xFD;
//        if((i!=79&&j==right[(i-35)/10])||(i==79&&j==right[5]))d=0xFD;
//        if((i!=79&&j==scan_mid[(i-35)/10])||(i==79&&j==scan_mid[5]))d=3;
////      }
//      if(i<60 && (j == Ring_left[i] || j== Ring_right[i]))
//          d=0xFD;


     if(i>34&&i<82&&(i%2==1))
      {

        if(j==all_out_left[(i-35)/2])d=0xFD;
        if(j==all_out_right[(i-35)/2])d=0xFD;

        if(j==all_out_scan_mid[(i-35)/2])
            if(((i-35)/2)==12)
                d=0xFD;
            else
                d=0;
      }



//          if(i==65){
//              if(j==left[3])d=0xFD;
//        if(j==right[3])d=0xFD;
//        if(j==scan_mid[3])d=0;
//          }
//            if(Cross_Flag==1&&i==65)
//      if(j==(-polt_x+94))
//        d=0xC8;




            uart_write_byte(UART_0,d);
    }
    uart_write_buffer(UART_0, end_cmd, sizeof(end_cmd));    //����
  }
  j=0;
}
float slope_k1(int x1,int y1,int x2,int y2)//��б��
{
    float x=0;
    x=(float)(x2-x1)/(float)(y2-y1);//x=(x2-x1)*((y-y1)/(y2-y1))+x1;
    return x;
}
//***************ֱ��(����)***************************//
void straight_line(void)
{
    int straight_left=0,straight_right=0,straight_lvalue=0,straight_rvalue=0;


//    oled_show_int(0, 2,straight_left , 4);
//    oled_show_int(0, 6,straight_right , 4);
    if(straight_count>-10)
        straight_count--;

//    straight_rvalue=(int)(slope_k1(all_out_right[21],77,all_out_right[8],51)*100);
//    straight_lvalue=(int)(slope_k1(all_out_left[21],77,all_out_left[8],51)*100);
//    if(out[100*COL+all_out_scan_mid[21]]>Threshold)
//    {
        straight_left=(int)(slope_k1(all_out_left[21],77,all_out_left[8],51)*100);
        straight_right=(int)(slope_k1(all_out_right[21],77,all_out_right[5],51)*100);
//    }
    if(straight_left>80&&straight_left<160&&straight_right<-80&&straight_right>-160)
        straight_flag=1;
    else
        straight_flag=0;

    if(straight_flag==1)
        straight_count=3;

    if(straight_count>0&&(Left_straight_EncoderCount+Right_straight_EncoderCount)>4000&&out[83*COL+all_out_scan_mid[21]]>Threshold-30)
            {straight_rush_flag=1;Bee_on();}
    else if(straight_count>0&&(Left_straight_EncoderCount+Right_straight_EncoderCount)>4000)
         {straight_rush_flag=2;Bee_on();}
    else
    {
        straight_rush_flag=0;
        Bee_off();
    }

//    oled_show_int(0, 2,out[100*COL+all_out_scan_mid[21]] , 4);
//    oled_show_int(0, 4,straight_flag , 4);
//    oled_show_int(42, 2,straight_count , 4);
//    oled_show_int(84, 2, (Left_straight_EncoderCount+Right_straight_EncoderCount), 4);

//    if()
}
void zhanai(void)
{
    static int8 zhi_count=0,zhi_conunt_1=0;
    int left_zhi=0,right_zhi=0;


//    left_zhi=(int)(slope_k1(all_out_left[break2_flag],(35+(break2_flag-1)*2),all_out_left[5],45)*100);
//                right_zhi=(int)(slope_k1(all_out_right[break2_flag],(35+(break2_flag-1)*2),all_out_right[5],45)*100);
//                oled_show_int(42, 4, left_zhi, 4);
//                oled_show_int(42, 6,right_zhi , 4);
    if(break_flag>15&&break2_flag>15&&fabs(angle_value)<45&&zhi_flag==0)//&&break_flag<23
    {//&&fabs(all_out_value[break2_flag])<45
        if(dl1a_distance_mm<1000&&zhi_count<10)
            zhi_count++;
        else if(dl1a_distance_mm>1000)
            zhi_count=0;

        if(zhi_count>0&&all_out_flag[5]==1)
        {
            left_zhi=(int)(slope_k1(all_out_left[break2_flag],(35+(break2_flag-1)*2),all_out_left[5],45)*100);
            right_zhi=(int)(slope_k1(all_out_right[break2_flag],(35+(break2_flag-1)*2),all_out_right[5],45)*100);
//            oled_show_int(42, 4, left_zhi, 4);
//            oled_show_int(42, 6,right_zhi , 4);
        }
        if((left_zhi>100&&left_zhi<250)||(right_zhi<-100&&right_zhi>-250))
        {
            zhi_conunt_1++;
        }
        else
        {
            zhi_conunt_1=0;
        }
        if(zhi_conunt_1>0)//��ֹ��������
        {
            Bee_on();
            zhi_flag=1;
        }
    }
    if(zhi_flag==1)
    {
            zhi_value_flag=1;

            if(zhi_time>20)
        {
            zhi_flag=2;
            zhi_time=0;
            ringvlauemax=0;
        }

    }
    if(zhi_flag==2)
    {

        if(zhi_plush<4000)
        {
            zhi_value_flag=2;//ֱ��һ�ξ���
//            zhi_flag=3;
        }
        else
        {
//            zhian_saox_y();
            zhi_plush=0;
            zhi_flag=3;
            zhi_value_flag=3; //��ʼ���
        }



    }
//    if()


    if(zhi_flag==3)
    {
        zhian_saox_z();//�ϰ�������ɨ��
//        zhian_saox_y();//�ϰ�������ɨ��   &&Width_count_num>1500
        if(fabs(Ring_Roll)>35&&((ForkRoad_En==2&&Width_count_num>1500)||(ad_value[0]>200||ad_value[1]>200||ad_value[2]>150||ad_value[4]>200||ad_value[5]>200)))//Width_count_num>24001500
                {
                    zhi_flag=0;
                    zhi_value_flag=0;
                    zhi_time=0;
                    zhi_count=0;
                    zhi_plush=0;
                    Ring_Roll=0;
                    Bee_off();
                    ForkRoad_En=0;
                    Memory_En=0;
                    yuansujis();//����·
                }
    }
//    oled_show_int(0, 2,(int)zhi_plush , 5);
//    oled_show_int(0, 4,(int)angle_value , 4);
//    oled_show_int(42, 0,(int)zhi_flag , 4);
//    oled_show_int(0, 0,(int)Ring_Roll , 4);
//    oled_show_int(84, 2, (int)White_average,4);
//    oled_show_int(84, 4, (int)Width_count_num,4);
}
void zhian_saox_z(void)//�ϰ�������ɨ��
{
    int i=0;
       zhi_left1=-1;
       zhi_right1=-1;
       if(zhi_time>70)//60
       {
           if(break_flag>19)
               value[3]=all_out_value[15]+10;
           else
                   value[3]=-10;
           ForkRoad_En=2;
   //        Bee_on();
       }
       else
       {
             value[3]=-40;//-30
       }


}
void zhian_saox_y(void)//�ϰ�������ɨ��
{
    int i=0;
    zhi_left1=-1;
    zhi_right1=-1;
    if(zhi_time>70)//60
    {
        if(break_flag>19)
            value[3]=all_out_value[15]-10;
        else
                value[3]=10;
        ForkRoad_En=2;
//        Bee_on();
    }
    else
    {
          value[3]=40;//30
    }
}
void duanlu(void)
{
    //������
//    all_out_left
//    all_out_right
//    all_out_value
//    out[81*COL+a]
    if(break2_flag>14&&break2_flag<20&&out[(2*break2_flag+3+35)*COL+all_out_scan_mid[break2_flag]]<Threshold
            &&out[(2*break2_flag+3+35)*COL+all_out_scan_mid[break2_flag]-5]<Threshold
            &&(all_out_right[break2_flag]-all_out_left[break2_flag])<17
            &&(all_out_left[break2_flag-2]<all_out_left[break2_flag])&&(all_out_right[break2_flag-2]>all_out_right[break2_flag])
            &&duanflag==0&&dl1a_distance_mm>2000)
    {
        duanflag=1;
        Bee_on();
    }//�����
    if(break2_flag>14&&break2_flag<20&&out[(2*break2_flag+3+35)*COL+all_out_scan_mid[break2_flag]]<Threshold
            &&out[(2*break2_flag+3+35)*COL+all_out_scan_mid[break2_flag]+5]<Threshold
            &&(all_out_right[break2_flag]-all_out_left[break2_flag])<17
            &&(all_out_left[break2_flag-2]<all_out_left[break2_flag])&&(all_out_right[break2_flag-2]>all_out_right[break2_flag])
            &&duanflag==0&&dl1a_distance_mm>2000)
    {
        duanflag=1;
        Bee_on();
    }//�����
    if(break2_flag>12&&break2_flag<16&&out[(2*break2_flag+3+35)*COL+all_out_scan_mid[break2_flag]]<Threshold
            &&(all_out_right[break2_flag]-all_out_left[break2_flag])>40&&dl1a_distance_mm>2000&&duanflag==0)
    {
        duanflag=1;
        Bee_on();
    }//ֱ��

    if(duanflag==1&&Width_count_num>2000&&zhi_plush>10000)
    {
        duanflag=2;
        zhi_plush=0;
    }
    if(duanflag==2&&zhi_plush>5000)
    {
        duanflag=0;
        Bee_off();
        zhi_plush=0;
        yuansujis();//����·
    }

//    if(break2_flag>14&&break2_flag<20&&fabs(angle_value)<45&&duanflag==0&&White_average>65&&Width_count_num<2000)
//    {
//
//    }
}
void podao(void)//�µ�
{
    static int8 po_count=0;
    if(Ramp_Flag==0&&Width_count_num>2100&&dl1a_distance_mm<450&&break_flag>17)//&&break2_flag>18
    {
        po_count++;
        if(po_count>2)
        {
            Ramp_Flag=1;
            Bee_on();
            AGLE=Balance_Bal_AG_Angle;
        }
//
    }
    else
                po_count=0;
    if(Ramp_Flag==1&&AGLE<Balance_Bal_AG_Angle+10&&zhi_plush>5000)
    {
        Ramp_Flag=2;
    }
    if(Ramp_Flag==2&&zhi_plush>20000)
    {
        zhi_plush=0;
        Ramp_Flag=0;
        po_count=0;
        Bee_off();
        Ramp_En=0;
        yuansujis();//����·
    }
//    if()
}
//void Destination_Y(void)
//{
//   if(bmx_flag==0&&Width_count_num>2200&&break2_flag<16&&break2_flag>8&&break_flag>17
//     &&((repeated_line_r==0&&Lost_line_r==0)||(repeated_line_l==0&&Lost_line_l==0)))
//       //&&((repeated_line_r=0&&Lost_line_r=0&&Lost_line_l>0)||(repeated_line_l=0&&Lost_line_l=0&&Lost_line_r>0))
////    if(0)
//    {
//       if((all_out_right[break2_flag+4]-all_out_left[break2_flag+4]<10&&all_out_flag[break2_flag+4]==1)
//           ||(all_out_right[break2_flag+5]-all_out_left[break2_flag+5]<10&&all_out_flag[break2_flag+5]==1)
//           ||(all_out_right[break2_flag+3]-all_out_left[break2_flag+3]<10&&all_out_flag[break2_flag+3]==1)
//           ||(all_out_flag[break2_flag+4]==3&&all_out_left[break2_flag]<all_out_left[break2_flag+4]&&all_out_left[break2_flag+7]<all_out_left[break2_flag+4])    ){
//       bmx_flag=1;
//        Bee_on();
//       }
//    }
//}
void Destination_Y(void)
{
  int i=0;
    static uint8 Destination_count;
    if(Cross_Flag==0&&Ring_Flag==0&&Ramp_Flag==0&&ForkRoad_Flag==0)
    {
                        for(int i=70;i<120;i++)
                        {
                                    if(Reversal==0)
                                    {
                                            if((out[70*COL+i]+out[70*COL+i+1]-out[70*COL+i-1]-out[70*COL+i-2]>40)&&i<180&&out[70*COL+i-2]<black
                                                ||(out[68*COL+i]+out[68*COL+i+1]-out[68*COL+i-1]-out[68*COL+i-2]>40)&&i<180&&out[68*COL+i-2]<black
                                                ||(out[65*COL+i]+out[65*COL+i+1]-out[65*COL+i-1]-out[65*COL+i-2]>40)&&i<180&&out[65*COL+i-2]<black)
                                            {
                                                    Reversal=1;
                                                    Reversal_num++;
                                                    continue;
                                            }
                                    }
                                    else
                                    {
                                            if((out[70*COL+i]+out[70*COL+i-1]-out[70*COL+i+1]-out[70*COL+i+2]>40)&&i<180&&out[70*COL+i+2]<black
                                                ||(out[68*COL+i]+out[68*COL+i+1]-out[68*COL+i-1]-out[68*COL+i-2]>40)&&i<180&&out[68*COL+i-2]<black
                                                ||(out[65*COL+i]+out[65*COL+i-1]-out[65*COL+i+1]-out[65*COL+i+2]>40)&&i<180&&out[65*COL+i+2]<black)
                                            {
                                                    Reversal=0;
                                                    Reversal_num++;
                                                    continue;
                                            }
                                    }
                        }
                        if(Reversal_num>12)
                        {
//                            Destination_Flag=1;
                            Bee_on();
                            bmx_flag=1;
                        }
                        else
                        {
                            Reversal_num=0;
                        }
                        if(bmx_flag==1)
                    {
                        if(zhi_plush>7000)
                        {
                            Reversal_num=0;
                            bmx_flag=0;
                            zhi_plush=0;
                            Bee_off();
                            Ring_En=0;
                            Cross_En=0;
                            Ramp_En=0;
                            ForkRoad_En=0;
                            dm_offset_flag=1;
                            Ramp_count++;
                            bmx_count++;
                        }
                        else
                        {
                            Destination_LineFind_Y();       //����������ɨ�ߣ����ұ���
                        }
                    }
    }
}
void Destination_Z(void)
{
  int i=0;
    static uint8 Destination_count ;
    if(Cross_Flag==0&&Ring_Flag==0&&Ramp_Flag==0&&ForkRoad_Flag==0&&bmx_flag==0)
    {
                        for(int i=60;i<120;i++)
                        {
                                    if(Reversal==0)
                                    {
                                            if((out[69*COL+i]+out[69*COL+i+1]-out[69*COL+i-1]-out[69*COL+i-2]>40)&&i<180&&out[69*COL+i-2]<black
                                                ||(out[68*COL+i]+out[68*COL+i+1]-out[68*COL+i-1]-out[68*COL+i-2]>40)&&i<180&&out[68*COL+i-2]<black
                                                ||(out[67*COL+i]+out[67*COL+i+1]-out[67*COL+i-1]-out[67*COL+i-2]>40)&&i<180&&out[67*COL+i-2]<black)
                                            {
                                                    Reversal=1;
                                                    Reversal_num++;
                                                    continue;
                                            }
                                    }
                                    else
                                    {
                                            if((out[69*COL+i]+out[69*COL+i-1]-out[69*COL+i+1]-out[69*COL+i+2]>40)&&i<180&&out[69*COL+i+2]<black
                                                ||(out[68*COL+i]+out[68*COL+i+1]-out[68*COL+i-1]-out[68*COL+i-2]>40)&&i<180&&out[68*COL+i-2]<black
                                                ||(out[67*COL+i]+out[67*COL+i-1]-out[67*COL+i+1]-out[67*COL+i+2]>40)&&i<180&&out[67*COL+i+2]<black)
                                            {
                                                    Reversal=0;
                                                    Reversal_num++;
                                                    continue;
                                            }
                                    }
                        }
                        if(Reversal_num>12)
                        {
//                            Destination_Flag=1;
                            Bee_on();
                            bmx_flag=1;
                            Reversal=0;
                            Reversal_num=0;
                        }
                        else
                        {
                            Reversal_num=0;
                        }
    }
    if(bmx_flag==1)
                       {
                        if(bmx_count>=bmx_count_value-1){
                        for(int i=20;i<160;i++)
                            {
                                if(Reversal==0)
                                {
                                    if(((out[61*COL+i]+out[61*COL+i+1]-out[61*COL+i-1]-out[61*COL+i-2]>40)&&i<180&&out[61*COL+i-2]<black)
                                        ||((out[60*COL+i]+out[60*COL+i+1]-out[60*COL+i-1]-out[60*COL+i-2]>40)&&i<180&&out[60*COL+i-2]<black)
                                        ||((out[59*COL+i]+out[59*COL+i+1]-out[59*COL+i-1]-out[59*COL+i-2]>40)&&i<180&&out[59*COL+i-2]<black)
                                        )
                                    {
                                            Reversal=1;
                                            Reversal_num++;
                                            continue;
                                    }
                                }
                                else
                                {
                                    if(((out[61*COL+i]+out[61*COL+i-1]-out[61*COL+i+1]-out[61*COL+i+2]>40)&&i<180&&out[61*COL+i+2]<black)
                                        ||((out[60*COL+i]+out[60*COL+i-1]-out[60*COL+i+1]-out[60*COL+i+2]>40)&&i<180&&out[60*COL+i+2]<black)
                                        ||((out[59*COL+i]+out[59*COL+i-1]-out[59*COL+i+1]-out[59*COL+i+2]>40)&&i<180&&out[59*COL+i+2]<black)
                                       )
                                    {
                                            Reversal=0;
                                            Reversal_num++;
                                            continue;
                                    }
                                }
                            }
                        }
                                if(Reversal_num>12)
                                {
        //                            Destination_Flag=1;
                                    Bee_off();
                                    bmx_flag=2;
                                    zhi_plush=0;
                                }
                                else
                                {
                                    Reversal_num=0;
                                }
                           if(zhi_plush>10000&&bmx_count<bmx_count_value-1)//&&0
                           {
                               Reversal_num=0;
                               bmx_flag=0;
                               zhi_plush=0;
                               Bee_off();
                               Ring_En=0;
                               Cross_En=0;
//                               Ramp_En=0;
                               ForkRoad_En=0;
                               bmx_zy_flag=0;
                               dm_offset_flag=1;
                               Ramp_count++;
                               bmx_count++;
                           }
                           else
                           {
                               if(out[66*COL+20]>White_average-30&&bmx_zy_flag==0)
                                   bmx_zy_flag=1;
                               else  if(out[66*COL+165]>White_average-30&&bmx_zy_flag==0)
                                   bmx_zy_flag=2;
                               if(bmx_zy_flag==1)
                               Destination_LineFind_Z();       //����������ɨ�ߣ����ұ���
                               if(bmx_zy_flag==2)
                               Destination_LineFind_Y();
                           }
                       }
    if(bmx_flag==2)
    {
//        if(break2_flag<16&&(break_flag-break2_flag)<5&&break_flag>break2_flag&&Width_count_num<1600)
//        {
//            bmx_value_flag=1;
//        }
//        if(zhi_plush>200)
//            bmx_value_flag=1;
        if(zhi_plush>8500)//((break2_flag<14&&(break_flag-break2_flag)<5&&break_flag>break2_flag)||(break2_flag<2&&break_flag<13))&&
        {
            bmx_flag=3;
        }
    }
}
void Garage_Out_Z(void)
{
    if(Garageout_step==0)
    {
      scan_mid[0]=left[0]+30;//�����ұ���
    value[3]=COL/2-scan_mid[0];
            if((((out[40*COL+left[0]-30]+out[41*COL+left[0]-30]+
         out[42*COL+left[0]-30]-out[39*COL+left[0]-30]-
         out[38*COL+left[0]-30]-out[37*COL+left[0]-30]>40)
         &&out[39*COL+left[0]-30]+out[37*COL+left[0]-30]<2*black+10////////////////////�Ķ�
         &&out[40*COL+left[0]-30]+out[42*COL+left[0]-30]<2*white)))
            {
                Garageout_step=1;
        GarageEncoder_Flag=1;
                Bee_on();
            }
    }
    if(Garageout_step==1)
    {
      value[3]= - 30;
    }
}
/*******************************************��������*************************************************************/
void Garage_Out_Y(void)
{
        if(Garageout_step==0)
    {
        scan_mid[0]=right[0]-30; //���������
    value[3]=COL/2-scan_mid[0];
            if((((out[35*COL+right[0]+30]+out[36*COL+right[0]+30]+
         out[37*COL+right[0]+30]-out[34*COL+right[0]+30]-
         out[33*COL+right[0]+30]-out[32*COL+right[0]+30]>40)
         &&out[34*COL+right[0]+30]+out[32*COL+right[0]+30]<2*black+10////////////////////�Ķ�
         &&out[35*COL+right[0]+30]+out[37*COL+right[0]+30]<2*white)))
            {
                Garageout_step=1;
        GarageEncoder_Flag=1;
                Bee_on();
            }
    }
    if(Garageout_step==1)
    {
      value[3]= 30;
    }
}
void chuku(void)
{
    static uint8 chu_count=0;

//    if(break_flag>18)
//        chu_count++;
//    else
//        chu_count=0;
//    if(Garageout_step==0&&(out[61*COL+94]+out[62*COL+94]+out[63*COL+94]-out[64*COL+94]-out[65*COL+94]-out[66*COL+94])>60)
//    if((((out[40*COL+left[0]-30]+out[41*COL+left[0]-30]+
//             out[42*COL+left[0]-30]-out[39*COL+left[0]-30]-
//             out[38*COL+left[0]-30]-out[37*COL+left[0]-30]>40)
//             &&out[39*COL+left[0]-30]+out[37*COL+left[0]-30]<2*black////////////////////�Ķ�
//             &&out[40*COL+left[0]-30]+out[42*COL+left[0]-30]<2*white))&&Garageout_step==0)
    if(Garageout_step==0&&zhi_plush>3000)
    {
        Garageout_step=1;
        Bee_on();
    }
//    if(Garageout_step==1)
//    {
//        Garageout_step=2;
//        Bee_off();
//    }
    if(Garageout_step==1&&Width_count_num>1400&&break_flag>15)
    {
        Garageout_step=2;
        zhi_plush=0;
        Bee_off();
    }
}
void yuansujis(void)//Ԫ�ؽ����������·��
{
    jis_flag=1;
}
void  yuansujis_count(void)
{
    if(jis_plush>4000&&jis_flag==1)
        {
            ys_conut++;
            jis_flag=0;
            jis_plush=0;
            PID_flag=1;
        }
}
void ysduil(void)
{
    if(ys_sz[ys_conut]==0)//Բ��
    {
        Ring_offset(); //Բ��
    }
    else if(ys_sz[ys_conut]==1)//��·
    {
        duanlu();      //��·
    }
    else if(ys_sz[ys_conut]==2)//�ϰ�
    {
        zhanai();      //�ϰ�
    }
    else if(ys_sz[ys_conut]==3)//�µ�
    {
        podao();       //�µ�
    }
    else if(ys_sz[ys_conut]==4)//ʮ��
    {
        Cross_Contrl();//ʮ��
    }
    else if(ys_sz[ys_conut]==4)//������
    {
        Destination_Z();//��������Ѱ��
    }
//    if(Ramp_Flag==0&&GarageOut_En==0)
//                Threshold_value();
//            if(Cross_Flag==0&&Ramp_Flag==0&&zhi_flag==0&&duanflag==0&&bmx_flag==0&&Garageout_step==2&&jis_flag==0)
//                Ring_offset(); //Բ��
//            if(Ring_Flag==0&&Ramp_Flag==0&&zhi_flag==0&&duanflag==0&&bmx_flag==0&&Garageout_step==2&&jis_flag==0)
//                Cross_Contrl();//ʮ��
//            if(Ring_Flag==0&&Ramp_Flag==0&&zhi_flag==0&&Cross_Flag==0&&bmx_flag==0&&Garageout_step==2&&jis_flag==0)
//                duanlu();      //��·
//            if(Ring_Flag==0&&Cross_Flag==0&&duanflag==0&&bmx_flag==0&&Ramp_Flag==0&&Garageout_step==2&&jis_flag==0)
//                zhanai();      //�ϰ�
//            if(Ring_Flag==0&&zhi_flag==0&&duanflag==0&&bmx_flag==0&&Cross_Flag==0&&Garageout_step==2&&jis_flag==0)
//                podao();       //�µ�
//            if(Ring_Flag==0&&zhi_flag==0&&duanflag==0&&Ramp_Flag==0&&Cross_Flag==0&&Garageout_step==2&&jis_flag==0)
//                Destination_Z();//��������Ѱ��
}
//void find_out(void)
//{
//    int i=0,j=0,Nhang=0;
////
//    for(i=0;i<120;i++){
//        Nhang=i;
//        for(j=4;j<188-4;j++)
//        {
//            out3[Nhang*COL+i]=out[Nhang*COL+i]+out[Nhang*COL+i+1]+
//                    out[Nhang*COL+i+2]-out[Nhang*COL+i-1]-
//                    out[Nhang*COL+i-2]-out[Nhang*COL+i-3];
//        }
//        }
//
//}
